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ABSTRACT 


POSITIONING  OF  A  ROBOTIC  MANIPULATOR 
THROUGH  THE  USE  OF  VISUAL  FEEDBACK 

by 

MAURICE  THOMAS  O'DONNELL 
University  of  New  Hampshire,  May,  1985 

A  system  for  positioning  a  general  purpose  industrial  robot  by 
means  of  information  extracted  by  a  computer  vision  system  is  described. 
The  specific  example  implemented  was  the  interception  of  a  moving  object 
by  a  robotic  manipulator  based  on  three  sequential  images  of  the  object. 
The  techniques  used  and  constraints  imposed  are  discussed.  The  robot 
used  is  an  industrial  G.E.  P-5  robot  with  five  degrees  of  freedom. 
Several  experiments  used  to  evaluate  the  various  components  of  system 
error  were  run.  The  results  of  these  experiments  were  compared  to  the 
behavior  of  a  linear  predictive  Kalman  filter  model  of  the  system.  A 
list  of  conclusions  is  presented  along  with  a  discussion  of  particular 
areas  where  improvements  on  this  system  can  be  made.  A  listing  of  the 
Fortran  programs  used  and  an  outline  of  the  camera-robot  coordinate 
system  calibration  process  are  included  in  the  appendices. 
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CHAPTER  I 


INTRODUCTION  ; 

The  underlying  objective  of  this  project  is  to  direct  the  movement  I 

of  a  robotic  manipulator  using  information  acquired  through  an  image 
acquisition  system.  As  an  example  of  such  a  movement,  a  robotic 
manipulator  is  driven  to  intercept  a  moving  object.  The  prediction  of 
the  object's  position  at  a  fixed  future  time  is  based  on  spacial  infor¬ 
mation  extracted  from  three  sequential  images  of  the  moving  object. 

For  the  most  part  in  today's  industrial  workplace  robots  are  per¬ 
forming  what  seems  to  be  rather  trivial  tasks.  That  is,  tasks  consisting 
of  maneuvers  that  can  be  played  back  over  and  over  again.  Possibly  this 
could  be  the  primary  reason  why  industry  has  been  the  ground  breaking 
area  for  use  of  robotic  manipulators.  This  is  because  a  majority  of 
industrial  tasks  are  clearly  repetitive  and  can  be  expressed  as  a 
i^quence  of  fixed  motions  [Horn  and  Ikeuchi,  1984].  Tasks  that  have 
been  .considered  tedious,  boring,  or  even  hazardous,  such  as  spray  paint- 

■  ■- ; ,  .'.elding,  arid  part  manipulation  have  been  the  perfect  opportunity  to 

■  -.rture  robot  technology.  The  particular  maneuvers  could  be  taught  to 
t'-e  robot  by  moving  the  robot  through  the  sequence  of  motions  while  the 
r-QDOt  control  facilities  would  store  the  sequence  in  some  type  of  memory. 

Then  -erely  by  running  the  stored  information  through  a  loop  the  robot 
would  be  ible  to  proceed  with  little  human  intervention. 

The  drawback  with  these  maneuvers  is  that  when  the  environment 
manges  in  some  way  the  system  breaks  down  rather  quickly.  For  example. 
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.levies,  sui.ii  as  a  controller,  a  robot,  a  burglar  alarm,  a  communications 
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rt-ien  discussing  trie  image  signal  in  monitors  or  television  screens, 
tnere  are  some  standard  features.  The  image  or  scene  on  a  television 
screen  is  due  to  the  presence  of  an  electron  beam  striking  a  phosphorous 
■.eating.  That  is,  the  gray  level  of  the  particular  point  in  the  scene 
depends  on  the  intensity  of  the  electron  beam  at  that  point.  It  is 
fairly  standard  for  the  electron  beam  to  be  moved  from  left  to  right  -.nd 
top  to  bottom.  The  term  'field'  has  been  applied  to  the  image  produced 
by  a  sequence  of  26  2  y  horizontal  scans  lines  at  a  rate  of  60  times  per 
second  [fianin,  1933],  By  interlacing  two  such  fields  into  a  single  image 
a  single  frame  is  generated.  The  frame  rate  is  therefore  approximately  3 
frames  per  second.  This  rate  prevents  the  appearance  of  flicker  in  tne 
scene  because  the  human  visual  system  is  able  to  sustain  the  image 
between  frames. 

Obviously,  the  ability  for  the  monitor  or  television  receiver  to 
reproduce  the  image  acquired  by  the  camera  depend  in  large  part  on 
wnetner  the  scanning  systems  of  the  camera  and  receiver  are  in  'sync', 
miis  synchronization  is  performed  by  the  generation  of  synthetic  video 
signals  by  the  camera  at  particular  timing  spots.  These  timing  spots 
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Figure  2.7  Dual  Moving  Mirror  linage  Plane  Scanner  [Castleman,  1979] 

The  current  technology  of  digitizers,  as  was  stated  before,  will 
take  an  analog  signal  and  quantize  it  through  an  analog-to-digital  con¬ 
verter  and  then  store  the  information  in  some  type  of  semiconductor 
memory.  For  the  reader  interested  in  the  A/D  and  quantization  infor¬ 
mation  Hoeschele's  text  [Hoeschele,  1968]  presents  a  complete  presentation 
on  tne  subject.  For  those  concerned  with  use  and  types  of  semiconductor 
memory  Muroga's  text  [Muroga,  1982]  presents  an  adequate  discussion  in 
this  area. 

At  this  point  in  tne  process  there  is  available  to  the  user  a  two- 
dimensional  representation  of  the  image  or  scene  located  in  a  semi¬ 
conductor  memory  which  can  now  be  processed  by  the  computer.  It  is 
■.bvious  tout  tne  cam  task  or  the  computer  in  the  vision  system  is  to 


13 

Some  popular  mechanical  scanning  devices  are  described  below. 

Mechanical  Drum:  With  this  device  the  image  is  wrapped  around  a 
cylindrical  drum.  The  image  is  then  rotated  past  a  stationary  aperture. 

The  aperture  is  moved  by  way  of  a  lead-screw.  After  an  entire  line  is 
digitized  the  lead  screw  is  repositioned.  This  process  is  repeated 
until  the  entire  image  is  scanned  [Figure  2.6]. 


IMAGE  ROTATING  DRUM 


Figure  2.6  Mechanical  "Drum"  Scanning  Mechanism  [Castleman,  1979] 

Flat  bed  scanner:  This  device  is  similar  to  the  drum  system, 
however  the  image  is  placed  on  a  flat  bed.  In  this  structure  either  the 
oed  or  the  image  is  repositioned  in  the  digitizing  process. 

Laser  scanner:  A  source  of  light  (laser)  and  a  mirror  configuration 
are  used  to  obtain  a  planar  image.  The  mirrors  are  connected  physically 
to  galvanometers,  which  are  driven  by  external  sources,  to  provide 
deflection  in  the  x  or  y  direction  [Figure  2.7], 


HORIZONTAL  CLOCK 


iyure  2.  4  CCD  A:  ray  (Line  Transfer)  [Ballard  and  Brown,  1982] 


HORIZONTAL  REGISTER  VIDEO  OUT 

Figure  2.5  CID  Array  [Ballard  and  Brown,  1982] 
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Figure  2.3  The  Vidicon  Camera  [Kunt,  1980] 

Solid  state  arrays:  As  a  result  of  present  technology  there  are  a 
number  of  solid  state  devices  used  for  image  formation  [Figures  2.4  and 
2.5]. 

Charged  coupled  devices  (CCD)  are  specially  manufactured  semi¬ 
conductor  transistors  which  are  photosensitive.  When  placed  in  an  array 
they  resemble  what  amounts  to  be  a  "bucket  brigade"  of  charge  in  a  shift 
register.  Through  a  series  of  clocking  pulses  the  charges  of  a  specific 
depletion  area  are  presented  to  the  output. 

Charge  injection  devices  (CID)  are  similar  to  a  CCD  array,  however 
a  charge  is  not  transferred  to  the  output.  The  specific  transistor  must 
be  addressed  in  order  for  the  data  to  be  read. 
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current  at  the  rear  of  the  tube  is  the  difference  between  the  scanning 
current  and  the  locally  absorbed  current. 

Image  dissector  tube:  Here  light  from  an  object  is  focused  on  a 
photocathode  which  converts  photon  energy  into  electron  flow.  Only  those 
electrons  emitted  from  a  specific  area  of  the  cathode  are  deflected  into 
an  aperture  and  reach  a  photo  multiplier.  The  spectral  range  of  the 
image  dissector  is  from  ultraviolet  to  infrared  (Figure  2.2). 


Figure  2.2  Image  Dissector  Camera  [Kunt,  1980] 

The  vidicon:  This  device  converts  the  energy  of  incoming  photons 
to  an  electron  flow  through  a  photoconducti ve  target.  The  target  is 
coated  witn  an  emulsion  whose  resistance  decreases  when  illuminated.  An 
electron  beam  is  scanned  on  the  back  side  of  this  target.  Due  to  the 
capacitance  the  charges  are  held  by  the  target.  The  result  is  a  capacitive 
current  which  is  used  as  the  video  signal  [Figure  2.3], 
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ation  of  these  two.  It  allows  for  the  object  to  be  illuminated 
by  a  moving  spot  and  sampled  through  a  moving  aperture.  Such 
a  system  is  obviously  complex  and  so  has  had  limited  use 
[Castleinan,  1979]. 

Some  of  the  popular  electronic  scanning  devices  are  described  below. 

Image  orthicon  camera:  This  system  (Figure  2.1)  has  several 
sections.  Light  from  the  scene  is  focused  onto  a  photocathode.  The 


ELECTRON  IMAGE 


Figure  2.1  Image  Orthicon  Camera  [Kunt,1980] 
photocathode  in  turn  emits  a  number  of  electrons  proportional  to  the  light 
intensity  toward  a  positively  charged  target.  This  second  target, 
which  consists  of  a  thin  glass  disk  with  a  wire  grid  facing  the  photo¬ 
cathode,  will  produce  a  secondary  emission  when  struck  by  the  electrons. 

As  a  result,  a  positive  charge  begins  to  build  up  on  the  photocathode 
side  of  the  second  target.  The  back  side  of  the  disk  is  continuously 
scanned  by  a  moving  electron  beam.  The  disk  absorbs  these  electrons  in 
a  neutralizing  process.  The  areas  of  high  intensity  will  use  a  large 
number  of  electrons  to  neutralize  this  part  of  the  target.  The  output 


3.  An  output  device  that  displays  either  an  output  image  or 
the  results  of  the  computer  processing. 

From  the  description  of  the  first  device  it  is  apparent  that  its 
purpose  is  to  take  an  analog  real  world  signal  and  hold  it  in  a 
recognizable  form  to  be  processed  by  a  computer.  This  initial  device  is 
known  as  an  image  digitizer.  There  are  several  key  elements  in  the 
digitizer.  First,,  it  must  possess  some  type  of  transducer  elements  which 
are  sensitive  to  the  energy  given  off  by  the  object  or  scene.  Secondly, 
it  must  be  able  to  quantize  the  incoming  information  into  a  form  capable 
of  being  processed.  Thirdly,  it  must  have  the  ability  to  isolate  the 
energy  at  particular  positions  of  the  scene  to  facilitate  investigations 
on  individual  picture  elements  known  as  pixels. 

It  is  through  certain  characteristics  that  one  is  able  to  catagorize 
a  particular  digitizer.  In  Ballard  and  Browns'  text  [Ballard  and  Brown, 
1982]  three  such  characteristics  are  presented: 

1)  Size  of  the  sampling  aperture  -  the  basic  spatial  resolution 
of  one  pixel . 

2)  Gray  Level  resolution  -  normally  expressed  as  the  number  of 
quantization  levels  between  black  and  white. 

3)  Scanning  technique  -  the  particular  technique  for  collecting 
the  light  energy.  There  are  basically  three  catagories  of 
scanning  techniques.  One  is  the  "scan-out"  method  where  the 
entire  object  is  illuminated  continuously  and  the  sampling 
aperture  is  positioned  so  that  the  energy  from  only  one  pixel 
is  received  by  the  sensor.  The  "scan-in"  method,  where  only 
one  area  of  the  object  is  illuminated  and  all  the  energy  is 
collected  by  the  sensor.  Finally  a  third  method  is  a  combin- 


CHAPTER  II 


IMAGE  ACQUISITION  AND  PROCESSING 

Digital  image  processing  is  an  area  of  study  that  cannot  be  placed 
under  any  one  scientific  or  engineering  discipline.  The  definitions  range 
from  simply  "...the  manipulation  of  images  by  computer..."  [Castleman, 
1979]  to  more  informative  statements  such  as  "...the  construction  of 
explicit,  meaningful  descriptions  of  physical  objects  from  images...." 
[Ballard  and  Brown,  1982],  It  is  obvious  that  within  this  second  quote 
lies  the  reason  for  most  of  the  activity  in  this  fairly  new  field. 

Computer  vision,  for  the  purposes  of  this  thesis,  is  a  term  used  synony¬ 
mously  with  digital  image  processing.  It  has  proved  to  be  a  very  useful 
tool  for  learning  additional  information  about  the  world  in  which  we 
live.  The  early  applications  of  this  field  which  included  space 
exploration  and  x-ray  technology  have  opened  doors  to  other  fields  in¬ 
cluding  robotics,  pattern  recognition,  image  graphics  and  so  on.  This 
chapter  presents  certain  facts  describing  techniques  and  equipment  used 
today  in  the  field  of  computer  vision.  After  this  presentation  the 
techniques  used  in  the  study  will  be  described. 

Computer  Vision  Systems 

There  are  basically  three  fundamental  parts  of  any  computer  vision 
system : 

1.  A  device  which  will  take  and  store  an  image. 

2.  The  digital  computer  which  performs  some  type  of  processing. 


In  Chapter  II  of  this  thesis  a  brief  introduction  to  specific 
aspects  of  computer  vision  is  presented.  The  elements  of  a  general  image 
acquisition  system  are  discussed.  The  specific  hardware  of  the  system 
used  in  this  research  is  briefly  described.  Finally,  the  image  pro¬ 
cessing  techniques  peculiar  to  this  system  are  explained. 

In  Chapter  III  a  discussion  of  robot  configurations  and  kinematics 
is  presented.  A  description  is  given  of  some  specific  details  of  the 
general  system.  The  transformation  from  the  attached  camera  coordinates 
to  the  manipulator  coordinates  is  derived.  This  chapter  ends  with  a  dis¬ 
cussion  of  the  specific  estimation  technique  used  in  the  example. 

In  Chapter  IV  the  system  performance  is  evaluated  by  means  of  three 
experiments  which  were  designed  to  quantify  imaging  error,  positioning 
error  and  total  system  error.  Also  introduced  is  a  simple  model  using  a 
Kalman  filter  that  will  serve  as  a  basis  for  examining  the  experimental 
data  of  the  system. 

In  Chapter  V  is  a  presentation  of  the  conclusions  of  this  research. 
Finally,  a  discussion  of  specific  improvements  on  the  system  is  presented 
along  with  an  indication  of  future  research  areas  that  are  possible  with 
this  system. 
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graphical  look  at  the  major  components  of  the  system.  The  four 
components  are: 

1)  The  Supervisory  Computer  receives  data  from  both  the  image 
acquisition  system  and  the  robotic  front  end  processor  and  controller. 
Through  the  use  of  a  high  level  language  the  supervisory  computer  can 
examine  the  information  concerning  the  existing  environment  and  then 
direct  a  movement  of  the  robotic  manipulator  in  response.  The  response 
is  delivered  to  the  robotic  front  end  processor  and  controller  by  means 
of  a  serial  communication  line. 

2)  The  Front  End  Processor  and  Controller  performs  two  main 
functions.  The  first  is  a  command  function  which  interprets  the  infor¬ 
mation  on  the  serial  line  as  either  data  or  instructions  to  be  accomplish¬ 
ed.  The  second  function  is  that  of  control  whereby  the  actual  robotic 
manipulator' s  trajectory  and  velocity  are  specified. 

3)  The  Image  Acquisition  System  provides  digital  images  of  the  area 
directly  below  the  end-effector  of  the  robot.  These  images  basically 
can  act  as  a  window  on  this  area  which  when  examined  in  sequence  provide 
dynamic  information  about  this  particular  scene. 

4)  The  Robotic  Manipulator  is  the  structure  that  is  being  directed 
by  the  system. 

The  example  used  to  test  the  robotic  system  developed  in  this  thesis 
is  a  simple  linear  tracker.  The  change  in  the  environment  that  is  being 
observed  is  a  moving  object.  The  system  monitors  this  object  and  makes 
an  estimate  about  the  object's  position  at  some  point  in  the  future. 

Once  the  future  position  is  calculated  the  system  derives  the  necessary 
information  to  direct  the  robotic  manipulator  to  intercept  this  object 
at  the  future  point  in  time. 


With  the  increased  demand  for  intelligent  robots  the  applications 
will  most  certainly  bring  them  out  of  the  industrial  area  into  new 
expanding  fields  such  as  space  exploration  and  deep  sea  mining.  Possibly 
any  area  that  might  present  a  hazard  to  man  while  necessitating  a  de¬ 
cision  being  made  will  offer  new  areas  of  use  for  the  intelligent  robot. 

It  turns  out  that  "computer  vision,  the  collection  of  techniques... 
for  obtaining  measurements  and  inferences  from  images,  appears  to  offer 
the  richest  source  of  sensory  information  for  intelligent  robotic 
manipulation  in  the  greatest  number  of  environments"  [Hall  et  al ,  1982]. 
The  need  for  vision-equipped  robots  is  seen  when  estimations  are  presented 
that  a  quarter  of  all  industrial  robots  will  be  equipped  with  some  form 
of  vision  system  [Miller,  1984]. 

In  this  research  an  attempt  at  establishing  an  intelligent  robot 
system  is  carried  out.  The  block  diagram  seen  in  Figure  1.1  reveals  a 


FRONT-END  PROCESSOR 

SUPERVISORY  COMPUTER  and  CONTROLLER 


IMAGE  ACQUISITION 
SYSTEM 


Figure  1.1  System  Block  Diagram 


These  reasons,  and  additionally  the  fact  that  numerous  other  tasks 
could  benefit,  have  led  to  the  continuing  development  of  intelligent 
robots.  Research  in  the  areas  of  transports  ion ,  bin-picking,  assembly, 
and  quality  control  has  been  on-going  for  the  last  few  years.  The 
problems  involved  with  transporting  items  from  place  to  place  in  a  "blind" 
workplace  have  been  stated  already.  The  ability  of  an  intelligent  robot 
to  overcome  these  problems  could  help  to  avoid  production  loss  due  to  down 
time,  damage  to  equipment  and  most  importantly,  injuries  to  human 
operators . 

Bin-Picking  is  a  term  that  is  applied  in  robotics  to  a  situation 
where  a  number  of  objects  are  in  close  proximity  and  the  system  is  able  to 
delineate  a  particular  object  and  then  to  maneuver  the  manipulator  in  such 
a  ,vay  as  to  grasp  and  transport  the  object. 

Assembly  is  a  term  applied  to  a  technique  whereby  a  robot  can  grasp 
several  ODjects  in  a  specific  order  and  then  orient  them  with  reference 
to  eacn  other  in  some  predetermined  fashion.  For  example,  putting  a  nut 
onto  the  end  of  a  bolt  or  a  peg  into  a  hole.  These  common  tasks  should 
not  be  belittled  because  of  the  complexities  involved  in  directing  such 
manipulations.  A  simple  assembly  task  may  involve  several  very  difficult 
subtasks,  such  as,  pattern  recognition,  bin-picking,  and  properly  align¬ 
ing  the  individual  objects. 

Finally,  Qual i ty  Control ,  which  is  a  repetitive  examination  of  manu¬ 
factured  parts  seems  to  be  an  excellent  application  for  an  intelligent 
robot  equipped  with  a  vision  capability,  thus  freeing  the  operator  for 
a  more  responsible  position  with  much  more  satisfaction.  This  task 
may  run  the  gamut  from  inspection  of  integrated  circuit  boards  to  paint 
jobs  on  the  body  of  a  new  automobile. 


j  ♦  an  ubsta..  '•<•  ■,.!•••  •  *  Jett*  mined  path  of  the  manipulator, 

there  is  no  way  to  ,»•  ;:’'ab!c''i  ur  to  avoid  the  collision.  With 

these  "non- in  tel  1 i jent '  robots  tne  orientation  and  initial  position  of 
the  manipulated  object,  the  path  of  movement  and  the  final  position  must 
be  accomplished  to  very  close  tolerances. 

It  is  obvious  that  a  large  number  of  tasks  exist  that  are  particularly 
suited  to  application  of  so-called  "intelligent"  robots.  These  are  robots 
equipped  with  transducers  so  that  changes  in  the  environment  can  be  de¬ 
tected  that  may  cause  the  system  to  falter.  These  robots  should  be 
equipped  to  compensate  for  these  contingencies. 

Some  authors  are  proposing  that  future  industries  will  strive  to 
improve  the  organization  of  the  workplace.  This  movement  is  working  to 
relieve  any  possibility  that  changes  in  the  environment  detrimental  to 
system  performance  occur.  This  trend  is  in  response  to  the  fact  that  for 
non-intel 1 igent  robots  all  positions  and  orientations  must  be  repetitive. 
Simple  tasks  such  as  moving  objects  from  place  to  place  presuppose  the 
ability  of  the  robot  to  grasp  the  object  properly  each  time.  Of  course 
this  ability  is  dependent  on  the  position  and  orientation  of  the  object. 

The  proponents  of  this  trend  are  advocating  that  storage  structures  for 
objects  be  carriers  and  pallets  rather  than  bins.  At  the  present  time 
much  research  is  devoted  to  designing  devices  that  will  perform  the 
positioning  and  orientation  tasks  on  objects  of  all  sizes.  There  are  a 
number  of  drawbacks  that  are  evident  in  such  a  trend.  First  there  is  a 
manufacturing  and  design  cost  for  these  new  devices.  Second,  if  an 
item  is  modified  in  any  way  the  orientation  device  must  also  be  modified. 
Third, to  provide  orientation  and  position  of  parts  may  not  be  the  most 
economical  way  to  store  them.  Finally,  it  seems  an  impossible  task  to 
provide  for  all  possible  contingencies  in  any  environment. 


occur  at  the  end  of  each  horizontal  scan  when  the  camera's  scanning 
system  must  be  moved  back  from  right  to  left.  At  this  time,  a 
horizontal  blanking  signal  is  generated  by  the  camera  so  that  no  infor¬ 
mation  is  acquired  during  this  retrace.  In  addition  a  horizontal  sync 
pulse  is  generated  at  these  times,  basically,  the  presence  of  the 
synthetic  signals  provide  the  timing  information  for  the  monitor  to 
lock-in  on  the  active  video  signal.  There  is  also  a  vertical  blanking 
signal  that  marks  the  time  for  the  scanning  system  to  reset  itself  to  the 
top-left  position.  Because  of  the  time  involved  with  this  vertical  re¬ 
trace  only  about  480  out  of  525  horizontal  scan  lines  provide  information 
to  the  monitor. 


Static  Scene  Analysis 

At  this  point  we  are  concerned  with  the  data  as  it  exists  in  the 
acquisition  system.  It  can  be  viewed  as  an  array  of  discrete  picture 
elements,  whose  numerical  value  is  an  indication  of  its  shading  or 
gray  level.  The  array  of  pixels,  therefore,  taken  as  a  whole  is  a  dis¬ 
crete  approximation  of  the  initial  scene  or  object.  Through  the  use  of 
hardware  and  software  algorithms  the  computer  vision  system  "...extracts 
pertinent  information  from  the  image  data.  This  module  in  essence 
performs  the  task  of  isolating  'interesting'  areas  for  further 
analysis"  [Computer,  Vol .  13,  1980]. 

It  is  this  idea  that  leads  to  the  next  area  of  discussion.  What 
types  of  information  processing  can  be  performed  on  these  distinct 
arrays?  It  is  this  extraction  of  data  from  the  frame  of  pixels  that  is 
given  the  title  of  scene  analysis.  The  additional  feature  of  temporal 
information  extraction  is  placed  under  the  title  of  dynamic  scene 


analysis.  There  are  a  number  of  papers  that  give  a  good  idea  of  this 
lively  field  of  investigation  which  are  listed  in  the  reference  area  at 
the  end  of  this  thesis. 
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The  authors  of  several  articles  use  different  terminology  to  del i - 
j  neate  the  methods  of  data  extraction.  However,  the  methods  themselves 

are  common  to  a  majority  of  the  authors. 

The  first  method  of  interest  is  concerned  with  improvement  of  image 
j  clarity.  This  area  of  study  is  known  as  image  enhancement.  Histogram 

i 

transformation  is  a  popular  approach  to  image  enhancement.  As  the  name 
implies  the  technique  involves  manipulating  the  gray  level  histogram  of 
I  an  image  [Hummel,  1975].  A  gray  level  histogram  is  simply  a  graphical 

representati on  displaying  the  frequency  of  occurrence  of  the  individual 
gray-levels  within  a  particular  image.  There  are  several  ways  to  use 
•  the  histogram  for  enhancement.  The  most  popular  technique  is  through 

j 

thresholding  the  grey-level  histogram  by  making  a  subjective  decision 
about  the  threshold  value  which  will  highlight  particular  objects, 
j  For  example,  a  common  approach  is  to  form  a  binary  image.  This  is  done 

by  cnanging  all  gray  levels  below  a  specific  threshold  to  black  and 
those  above  to  white. 

I  The  second  technique  is  known  as  histogram  equalization.  This 

method  tries  to  stretch  the  initial  histogram  consisting  of  n  gray  levels 
into  a  new  histogram  of  p  gray  levels  (p>n).  The  result  of  this  tech- 
l  nique  improves  contrast  and  therefore  facilitates  individual  object 

recognition  [Hummel,  1975]. 

The  third  technique  used  to  manipulate  the  histogram  is  in  histogram 


hyperbol ization.  This  method  tries  to  transform  the  histogram  of  dis¬ 
played  brightness  levels  by  producing  a  uniform  distribution  of  perceived 


brightness  levels.  "All  pictures  processed  in  this  way  have  been  con¬ 
sistently  considered  of  superior  intelligibility  than  their  histogram 
equalized  counterparts . "  [Frei  ,  1977], 


Along  with  enhancement  there  are  a  number  of  types  of  image  pro¬ 
cessing  techniques  available  to  extract  spacial  properties  from  a 
scene.  The  first  method  here  is  called  template  matching  where  a  pixel- 
by-pixel  comparison  is  performed  on  one  (usually  "live")  image  with 
another  (stored)  image  to  be  used  as  a  reference.  At  times  an  operator 
known  as  a  "template"  is  used  to  extract  or  detect  a  particular  sub¬ 
image.  The  template  is  placed  at  several  offsets  on  the  initial  image 
and  a  correlation  is  performed.  The  point  of  maximum  match  is  determined 
to  be  the  sub-image  under  investigation  [Computer,  1980]. 

Several  spatial  properties  can  be  extracted  by  the  second  method  of 
segmenting  the  image  into  meaningful  entities.  This  is  usually  performed 
by  the  approach  known  as  edge  detection.  An  edge  is  defined  as  an  area 
in  an  image  where  local  gray  levels  are  changing  rapidly.  Through  the 
use  of  an  edge  operator  the  presence  of  this  edge  can  be  detected.  "The 
unifying  feature  of  ...  edge  operators  is  that  they  compute  a  direction 
...  of  maximum  gray-level  change  and  a  magnitude  describing  the  severity 
of  this  change"  [Ballard  and  Brown,  1983].  Figure  2.8  is  retrieved  from 
Robinson's  paper  on  gradient  masks  [Robinson,  1971]  and  presents  several 
common  edge  operators  and  their  directional  sensitivity.  The  major 
problem  with  these  simple  operators  is  that  they  also  respond  to  noise 
areas  within  the  initial  image.  As  a  result  a  technique  called  edge 
relaxation  is  employed  to  improve  the  edge  operator  measurement  by 
basing  some  measurements  on  the  existence  of  neighboring  edges.  For 
example,  the  existence  of  two  strong  edges  within  the  vicinity  of  a 
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Figure  2.8  Examples  of  Compass  Gradient  Masks 
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relatively  weak  edge  may  give  more  credence  to  the  existence  of  this 
weak  edge  rather  then  to  the  possibility  that  it  is  just  a  noise  pattern 
in  that  area.  The  next  step  in  this  technique  would  be  to  somehow  group 
these  edges  into  logical  objects.  This  technique  therefore  assumes  that 
there  is  enough  background  information  to  specify  what  object  a  group 
of  edges  represents. 

Another  technique  for  segmentation  of  an  image  tries  to  overcome 
this  problem.  The  method  is  known  as  region  growing.  It  initially 
divides  the  image  into  basic  areas  either  by  grouping  identical  pixel 
values  or  simply  by  dividing  the  image  into  small  regions.  These  dis¬ 
tinct  groupings  or  regions  are  then  merged  together  on  the  basis  of 
"similarity",  where  this  criterion  would  be  different  for  each  system. 
There  are  several  problems  encountered  with  this  technique.  "Problems 
can  arise  in  the  selection  of  initial  regions,  and  in  selecting  the 
merging  criteria"  [Ohlander  et  al.,  1978]. 

All  the  methods  mentioned  thus  far  have  been  concerned  with  mani¬ 
pulating  static  frames  in  order  to  derive  specific  data  with  which  to 
identify  the  existence  of  objects,  to  find  their  dimensions,  and  possibly 
to  classify  them  based  on  some  criterion.  By  adding  the  dimension  of 
time  we  enter  the  field  of  dynamic  scene  analysis  which  obviously  yields 
additional  information.  This  new  information  holds  the  possibility  for  a 
number  of  interesting  applications. 

namic  Scene  Analysis 

In  looking  at  the  dynamic  case  it  is  easy  to  concur  with  Martin  and 
Aggarwal  in  recognizing  that  "...a  'dynamic  image1  is  a  sequence  of 
static  images  ...  with  a  given  or  assumed  time  function  relating  the  order 


and  elapsed  interval  between  elements  of  the  sequence"  [Aggarwal  and 
Martin,  1978],  Obviously  when  the  time  factor  is  introduced  we  can  view 
groups  of  images  which  have  similarities  as  well  as  differences.  The 
authors  proceed  to  demonstrate  the  idea  that  "...  a  dynamic  image  analysi 
system  must  be  able  to  separate  the  constancies  from  the  changes,  and 
be  able  to  separate  the  interesting  changes  from  the  noisy  ones" 

[Aggarwal  and  Martin,  1978]. 

Within  the  area  of  dynamic  scene  analysis  the  idea  of  motion 
detecti on  is  one  of  the  most  researched.  As  the  name  implies  this  study 
deals  with  the  ability  to  recognize  and  specify  spacial  changes  by 
studying  objects  in  motion.  In  a  number  of  techniques  motion  detection 
is  achieved,  but  the  ability  to  gain  information  about  specific  features 
is  lost.  The  earliest  research  in  this  area  dealt  with  the  detection 
and  measurement  of  cloud  motion  from  satellite  photographs  [Leese  et  al , 
1970],  One  of  the  approaches  in  these  studies  is  to  divide  an  initial 
image  into  sections  and  then  to  correlate  these  sections  with  related 
areas  in  the  following  image.  The  maximum  cross-correlation  coefficient 
is  interpreted  as  a  match  for  that  section.  As  a  result,  the  centers  of 
the  two  sections  are  connected  with  a  motion  vector.  The  second  tech¬ 
nique  that  seems  to  be  popular  is  the  binary  thresholding  technique. 

With  this  method  an  image  is  divided  into  two  gray  levels.  The  dividing 
point  is  chosen  so  that  the  boundary  of  cloud  formation  is  evident.  The 
next  step  is  to  match  each  cloud  formation  to  a  formation  in  the  follow¬ 
ing  image.  Obviously  the  drawbacks  to  these  two  techniques  are  that 
they  assign  a  motion  vector  to  a  section,  not  to  any  feature  within  the 


section. 


Another  technique  that  is  popular  for  an  indication  of  change  be¬ 
tween  two  images  is  a  simple  subtraction  technique.  If  two  images  have 
been  aligned  and  one  image  is  subtracted,  pixel  by  pixel,  from  the  other, 
a  resulting  image  will  yield  gray  levels  in  areas  where  there  are  changes. 
Again  this  technique,  though  straightforward,  has  some  inherent  problems. 
The  features  within  the  area  of  change  are  still  not  specified,  and  the 
presence  of  uncorrelated  noise  between  images  may  appear  as  meaningful 
changes.  When  using  any  of  the  methods  described  one  must  remain  aware 
of  the  limitations  of  the  individual  techniques  and  try  to  compensate  for 
them  in  the  particular  applications. 


Dment  Used 


the  Studs 


In  performing  the  image  processing.  Image  Technology  Inc.'s  digital 
picture  acquisition  system  was  used  in  conjunction  with  a  PDP  11/60 


Figure  2.9  Image  Technology,  Inc.  Hardware 
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digital  computer.  Figure  2.9  shows  a  graphical  representation  of  how  the 
system  is  constructed.  The  system  as  depicted  has  the  ability  of  pro¬ 
cessing  information  from  two  cameras.  The  multiplexer  merely  selects  the 
appropriate  channel.  The  equipment  digitizes  the  video  signal  in  real 
time.  This  rea1  time  capability  is  due  to  the  use  of  TRW  TDC100TJ  flash 
analog-to-digital  converters. 

The  pixel's  digitized  value,  which  ranges  from  0  to  255,  acts  as  a 
pointer  to  a  particular  input  look-up  table  (LUT).  In  the  address  of 
the  LUT  will  be  stored  a  value  (from  0  to  255)  which  represents  a  specific 
gray  level  between  black  (0)  and  white  (255).  This  gray  level  value  is 
then  stored  as  the  picture  element  in  the  image  frame  buffer  memory. 

The  image  frame  buffer  memory  consists  of  256K  bytes  of  dynamic  ram 
constructed  as  a  512  x  512  array.  The  FB-512  board,  as  the  manufacturer 
calls  it,  is  capable  of  driving  3  analog  signals  (R  (red),  B  (blue),  G 
(green)).  From  the  figure  the  output  value  from  the  frame  buffer  again 
acts  as  a  pointer  to  an  output  look-up  table.  The  particular  address  of 
this  LUT  contains  a  value  from  0  to  255  which  represents  the  gray  level 
for  the  output  representation.  The  final  step  then  is  to  convert  the 
digital  value  of  gray  level  to  an  analog  signal.  The  equipment  here  is 
the  TRW  TDC1016J-8  highspeed  digital-to-analog  converter.  The  signal 
is  then  processed  by  additional  circuitry  to  form  a  composite  video 
signal  suitable  for  a  standard  TV  monitor. 

The  control  of  this  hardware  is  performed  through  a  number  of 
MACRO  subroutines  and  functions.  The  ability  to  call  these  subroutines 
and  functions  from  high-level  languages  makes  the  hardware  functions 
almost  invisible  to  the  user.  There  is  issued  a  short  pamphlet 
entitled  IMAGING  -  Basic  Driver  Programmer's  Manual  which  explains  the 
use  of  the  different  subroutines  and  functions. 
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Processing  Techniques  Employed 

By  the  appropriate  use  of  the  system  hardware  and  software  it  was 
possible  to  avoid  many  of  the  problems  presented  in  the  references. 

Since  this  research  was  not  primarily  concerned  with  image  processing  or 
motion  detection,  it  was  decided  to  simplify  all  aspects  of  object 
recognition.  As  a  result,  the  number  of  objects  in  the  field  is  limited 
to  one.  Also  the  object  of  interest  is  a  symmetrical  shape  (circle). 
These  two  restrictions  are  used  so  that  estimation  of  the  object's 
center  could  be  facilitated.  It  was  intended  to  establish  motion  de¬ 
tection  by  specifying  a  change  of  position  for  the  center  of  the  object. 
Furthermore,  by  appropriately  setting  the  input  LUTs  to  produce  a 
binary  image,  a  high  contrast  between  object  and  background  is  assured. 

The  purpose  behind  using  the  image  acquisition  system  is  to  extract 
information  about  the  movement  of  an  object.  The  information  should 
provide  enough  data  to  make  an  estimate  of  the  object's  position  at  some 
advanced  point  in  time. 

Image  Technology's  system  allows  for  a  rather  intricate  use  of  the 
frame  buffer.  The  technique  is  called  "zooming."  Zooming  enables  a  user 
to  select  an  active  video  window  within  the  frame  buffer  where  image 
acquisition  can  be  held  to  a  local  region.  The  user  has  the  option  to 
"zoom"  in  either  the  x  or  y  direction  or  in  both.  The  result  is  that 
the  dimension  of  the  applicable  direction  is  reduced  by  one-half.  The 
location  of  the  active  video  window  is  chosen  by  panning  the  upper  left 
corner  of  the  window  to  a  specific  x  location  and  then  scrolling  of  this 
point  to  a  specific  y  location  [Figure  2.10].  The  active  video  window 
may  therefore  take  on  any  one  of  four  two  dimensional  sizes. 


ACTIVE 

VIDEO 

WINDOW 


The  method  incorporated  in  this  study  is  to  zoom  in  both  the  x-and 
y-di rections .  This  technique  allows  for  the  storage  of  several  images  in 
the  frame  buffer  simultaneously.  This  technique  allows  for  the  processing 
of  images  to  be  carried  out  at  the  same  time.  Three  images  are  taken 
at  .25  seconds  apart.  Therefore,  by  examining  the  location  of  the 
center  of  the  objects  in  each  image  an  estimate  of  both  velocity  and 
direction  can  be  extracted  [Figure  2.11]. 


Figure  2.11  512  x  512  Frame  Buffer  with  Three  Images 

during  the  initial  stages  of  research  two  techniques  were  in¬ 
vestigated  to  determine  the  objects'  centers.  The  first  of  the  techniques 
employed  is  t nown  as  the  turtle  boundary  detector/follower  [Duda,  1973]. 
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The  second  method  is  a  very  simple  approach  that  can  be  called  the  cross¬ 
hairs  approach. 

Both  methods  require  finding  an  object's  boundary  point  as  a  start¬ 
ing  location.  To  avoid  searching  each  entire  section  (256  x  256),  a 
method  was  devised  to  conduct  the  search  in  a  grid-like  approach.  That 
is  the  search  was  conducted  by  examining  every  fifth  line  of  the  y 
direction  starting  from  the  top  of  each  section  and  working  down. 

The  "turtle"  method  was  derived  to  examine  the  boundary  of  an  object 
located  in  a  binary  image.  By  keeping  track  of  the  x  coordinates  of 
the  boundary  pixels,  y  coordinates  of  the  boundary  pixels,  and  the  total 
number  of  pixels  in  the  boundary,  the  centroid  of  the  object  may  be  found 
[Dubois,  1984],  The  turtle  method  consists  of  moving  a  "turtle"  around 
the  boundary  or  an  image  in  the  direction  determined  by  whether  the 
current  pixel  is  an  object  pixel  or  background  pixel.  If  the  turtle  is 
located  on  an  object  pixel,  it  will  advance  by  making  a  left-turn  as  re¬ 
ferenced  to  its  last  movement.  If  it  is  located  on  a  background  pixel, 
it  will  advance  by  making  a  right-turn  as  referenced  to  its  last  move¬ 
ment  [Figure  2.12].  By  referring  to  the  figure,  it  is  obvious  that 
several  pixels  may  be  entered  more  then  once.  Software  must  insure  that 
duplicate  information  is  avoided.  The  object's  centroid  specified  by  an 
x  and  y  coordinate  is  derived  through  two  simple  equations. 


xcenter 


^center 
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The  most  striking  problem  in  using  the  "turtle" 

to  noise.  In  using  this  technique  the  investigation 

object  on  a  white  background.  The  noise  was  visible 


=  number  of  boundary 
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examined  a  black 
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and  the  object  images.  Several  techniques  were  tried  in  order  to  reduce 
this  sensitivity.  The  first  attempt  consisted  of  subjectively  determining 
a  threshold  value  that  would  reduce  the  background  noise  to  a  tolerable 
level.  This  attempt  merely  reaffirmed  the  fact  that  the  turtle  method,  as 
presented,  was  unable  to  cope  with  any  background  noise.  The  second 
attempt  was  based  on  an  assumption  that  the  large  number  of  white  back¬ 
ground  pixels  might  be  causing  saturation  in  the  automatic  gain  control 
of  the  vidicon  camera.  This  attempt  consisted  of  using  the  imaging 
system  to  produce  a  negative  image  of  the  scene.  That  is,  pixels  that 
were  white  would  be  black  and  those  that  were  black  would  be  white.  This 
technique  also  did  not  reduce  the  noise  as  expected.  The  third  attempt 
used  a  white  object  on  a  black  background  to  test  if  the  noise  was 
contributed  by  the  system  hardware.  Again  the  results  revealed  that  the 
nGise  was  present  at  an  unacceptable  level.  At  the  same  time  these  tests 
were  being  performed,  the  second  method  was  used  and  performing  well  in 
the  noisy  environment.  As  a  result  this  second  method  was  chosen  for 
continued  tests. 

The  cross-hairs  method  was  basically  designed  for  use  with  sym¬ 
metrical  objects.  The  technique  examines  the  horizontal  chord  of  the 
object  at  the  boundary  point.  Because  of  the  symmetry  of  the  object, 
the  center  of  the  chord  is  chosen  as  the  initial  x  center  for  the  object. 
The  next  step  is  to  examine  the  diameter  of  'he  object  in  the  y-direction 
passing  through  the  x  center  location.  Due  to  symmetry  the  middle  of 
the  diameter  is  chosen  as  the  initial  y  center  for  the  object. 

Due  to  noise  and  inaccuracies  in  the  system  it  was  decided  to 
extract  additional  information  in  determining  the  center  coordinates. 

This  was  accomplished  by  examining  points  along  the  cross-hairs  of  the 
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Figure  2.12  Turtle  Movement 

object.  After  the  initial  center  coordinates  were  located  the  next  step 
was  to  move  +  1/4  diameter  in  the  x  direction.  At  this  point  two  chords 
were  examined  to  find  their  midpoint  in  the  y-direction.  Finally  an 
average  of  the  three  values  was  taken  and  used  as  the  final  y  center  co¬ 
ordinates.  A  similar  procedure  was  used  to  determine  the  x  center 
coordinate  [Figure  2.13]. 

This  technique  works  in  a  noisy  environment  by  simply  setting  a 


limit  on  the  minimum  size  object.  That  is,  if  the  initial  chord  or 
diameter  was  less  than  four  pixels  long,  the  object  was  discarded  as  a 
noise  pattern  and  the  search  continued. 
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The  conclusion  is  that  the  position  and  orientation  of  the  manipulator  can 
be  expressed  as  the  product  of  a  translation  matrix  and  an  orientation 
matrix. 

In  the  kinematics  used  for  this  thesis  the  positioning  matrix  was 
the  Cartesian  coordinate  translation  matrix  and  the  orientation  matrix 
specified  by  the  a,  o  and  n  unit  vectors. 

The  general  form  of  this  transformation  matrix  is  given  by: 


,  n  o  a  p  . 
x  x  x  px  i 


n  o  a  p  . 

■  y  y  y  y 


[1z  °z  az  pzi 


i  0  0  0  1_, 

Figure  3.9  shows  five  specific  examples  of  the  transformati on 
matrix  for  various  positions  of  a  body-attached  coordinate  system  in  a 
reference  frame.  A  particular  column  of  the  transformation  matrix 
expresses  the  orientation  of  the  corresponding  body-attached  coordinate 
axis  in  terms  of  the  reference  coordinate  axes  in  the  form 
(ix.  iz>  0)  .  That  is,  if  we  look  at  the  first  column  of  each  matrix 
in  the  figure,  the  orientation  of  axis  01)  in  the  reference  coordinate 
system  OXYZ  can  easily  be  seen.  Likewise,  the  axes  OV's  and  OW's  orienta¬ 
tion  can  be  determined  by  observing  the  2nd  and  3rd  column  vector  of  the 
T  matrix.  The  4th  column  matrix  locates  the  origin  of  the  revolving  co¬ 
ordinate  system  in  the  reference  coordinate  system  in  the  form 
(ix,  i  ,  iz,  1)*\  Similar  information  about  the  orientation  and  location 
of  the  fixed  reference  system  in  terms  of  the  rotating  coordinate  system 
can  be  obtained  by  merely  taking  the  inverse  of  the  T  matrix: 


about  the  z  axis  and  finally  a  translation  z  along  the  z  axis.  Therefore 
the  matrix  representing  such  a  position  is  given  by: 

Cyl  (z,i,r)  =  Trans  (0,0, z)  Rot(z,0  Trans  (r,0,0) 


Figure  3.7  Cylindrical  Polar  Coordinates  [Paul,  1 981  j 
In  Figure  3.3  the  specification  through  spherical  coordinates,  r,  ,  and 
.  corresponds  to  a  translation  r  along  the  z  axis,  followed  by  a  rotation 
about  the  y  axis  and  finally  a  rotation  *  about  the  z  axis.  In  this 
case  the  position  can  be  described  as: 

Sph  (  r )  =  Rot  (z,0  Rot  (y,.  )  Trans  (0,0, r) 


riguro  3.3  Spherical  Polar  Coordinates  [Paul,  1981] 
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Euler  (;,.■,„)  =  Rot(z , ; )  Rot(y,-)  Rot(z,J 

The  third  method  for  expressing  the  orientation  of  the  end-effector 
is  given  in  terms  of  the  roll,  pitch,  and  yaw  angles,  terms  commonly 
used  in  speaking  about  ships.  Figure  3.6  depicts  the  situation  corres- 


Fiqure  3.6  Roll,  Pitch,  and  Yaw  Coordinates  for  a  Manipulator 
ponding  to  a  rotation  ;  about  the  z  axis  (roll),  then  a  rotation  •  about 
the  y  axis  (pitch)  and  a  rotation  .  about  the  x  axis  (yaw).  The  general  )' 

rotation  matrix  is  given  as: 

RPY(;,v,v)  =  Rot(z ,;)  Rot(y,r)  Rot  (x,v) 

I 

In  addition  to  specifying  the  orientation  of  the  coordinate  frame 
of  the  end-effector,  a  position  for  the  origin  of  this  frame  must  be 
established.  In  the  first  method  a  vector  p  was  introduced  to  specify 
this  position  in  the  base  coordinates.  This  position  can  also  be  de¬ 
scribed  in  both  cylindrical  coordinates  and  spherical  coordinates.  In 
Figure  3.7  the  specification  through  cylindrical  coordinates  r,  t,  and  z 

I 

corresponds  to  a  translation  r  along  the  x  axis,  followed  by  a  rotation  <  ;■ 


The  orientation  and  position  of  this  hand  can  be  described  by  an 
attached  coordinate  frame  whose  origin  is  located  at  the  midpoint  between 
the  two  fingers.  This  origin  is  also  described  by  a  vector  p  whose  co¬ 
ordinates  are  expressed  in  the  defining  reference  frame.  There  are  three 
unit  vectors  which  describe  the  orientation  of  the  hand.  The  approach 
vector,  a,  is  defined  as  the  direction  from  which  the  hand  would  approach 
an  object.  The  orientation  vector,  o,  is  in  the  direction  specifying  the 
orientation  of  the  hand,  from  fingertip  to  fingertip.  The  normal  vector, 
n,  will  complete  the  right  handed  coordinate  system  and  is  specified  by 
crossing  the  o  vector  into  the  a  vector  [Paul,  1981]. 

There  are  a  number  of  ways  of  specifying  the  orientation  of  a 

manipulator  or  in  this  case  the  end-effector.  It  is  obvious  that  within 

the  general  matrix  there  are  only  a  few  values  that  afford  any  information 
For  example  the  bottom  row  will  be  three  zeros  and  a  one  depending  on 
whether  scaling  and  perspective  become  involved. 

The  first  method  presented  in  the  text  [Paul,  1981]  is  by  specifying 
the  three  vectors  a,  o  and  n  discussed  before.  These  three  vectors 
specify  the  orientation  of  a  coordinate  frame  whose  position  can  be  de¬ 
fined  in  a  number  of  ways  to  be  discussed  later.  The  constraints  on  this 

method  are  simply  that  the  a,  o  and  n  vectors  are  of  unit  magnitude,  and 

the  p  vector  describes  a  location  that  can  be  reached  by  the  manipulator. 

The  first  method  can  be  viewed  as  a  Cartesian  approach  because  orient 
ation  is  expressed  as  distances  along  these  three  axes.  The  second  and 
third  methods  are  expressed  as  a  set  of  rotations.  The  second  method, 
using  a  set  of  Euler  angles,  can  describe  any  orientation  in  terms  of  a 
rotation  ;  about  the  z  axis,  then  a  rotation  f  about  the  new  y  axis  y1, 
and  finally  a  rotation  ■,  about  the  new  z  axis  z"  [Figure  3.5].  In  this 
case  the  general  rotation  matrix  car  be  expressed  as: 


1  0  0  dx 

0  1  0  dy 

^"tran  ”  0  0  1  dz 

|0  0  0  1_ 

In  deriving  kinematics  equations  for  any  robot  the  main  consider¬ 
ation  is  that  of  orienting  and  positioning  the  end-effector  of  the  robot. 
For  the  purposes  of  this  thesis  the  end-effector  is  the  fifth  link  of 
the  robotic  manipulator.  Therefore  in  specifying  the  individual  elements 
of  the  general  transformation  matrix  °Tn  much  more  information  is  establish 
ed  than  merely  transforming  the  coordinates  of  a  vector  expressed  in  the 
final  link's  coordinate  system  into  the  base  coordinates. 

One  method  of  interpreting  the  general  matrix  is  by  specifying  three 
vectors  n,  o,  and  a  as  shown  in  Figure  3.4,  which  represents  the  end 
effector  of  a  robot. 


Figure  3.4  Orientation  Vectors  Associated  with  Manipulator  [Paul,  1981] 
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The  homogeneous  transformation  matrix  takes  the  form: 


Translation 

3x3  P3xl 
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Therefore  the  homogeneous  transformation  matrices  for  pure  rotation  are: 

T  0  0  0; 

0  cos  <  -sin,  0 
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The  homogeneour  matrix  for  pure  translation  transforms  the  origin  of  the 
body  attached  coordinate  system  which  has  been  translated  (no  rotation) 
into  the  reference  coordinate  system.  The  basic  homogeneous  translation 


matrix  takes  the  form: 


an  angle  i  =  90°.  This  transformation  can  be  expressed  as: 


P  =  R  p 
Kxyz  x,u  Kuvw 

Therefore  this  rotational  matrix  can  be  expressed  as: 
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Similarly  for  rotations  about  the  axes  0V  and  OZ  we  can  find  rotation 
matrices  R  and  R  respecti vely.  These  can  be  expressed  as 

y  ?  -  ^ . 
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By  knowing  these  3x3  matrices  we  can  decompose  complex  rotations  into 
their  basic  rotation  matrices  and  derive  the  complex  rotational  matrix  R. 

R  =  R  •  R  .  •  R, 

X,a  y,"  2  ,  V 

By  adding  an  additional  coordinate  to  the  position  vector 

p  =  (wp  ,  wp.  wp,,w)t 
xyz  '  rx  y  z  ' 

the  position  of  the  point  is  expressed  in  homogeneous  coordinates.  As  a 
result,  the  4x4  homogeneous  transformation  matrix  now  has  the  capability 
of  expressing  coordinate  system  rotation,  translation,  scaling,  and  per¬ 
spective.  The  homogeneous  matrix  is  composed  of  the  original  rotation 
matrix  in  addition  to  3  new  components: 

-  3x1  position  vector 

-  1x3  perspective  vector 

-  lxl  scaling  factor 
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p  =  i  •  p  =  i  •  i  p  +  1  • i  p  +  i  •  k  p 

Kx  x  ^uvw  x  uKu  x  Jvtv  x  wKw 

p  =  i  •  p  =  i  •  i  p  +  i  •  i  p  +  i  •  k  p 

Ky  y  Huvw  Jy  upu  Jy  *Vv  Jy  wKw 

p  =  i  •  p  =  k  •  i  p  +  k  •  i  p  +  k  •  k  p 

Kz  z  Huvw  z  uHu  z  JvKv  z  wHw 

These  three  equations  can  be  expressed  in  matrix  form: 
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and  so  the  transformation  matrix  A  that  relates  the  body  attached  coordi¬ 
nate  system  to  the  reference  coordinate  system  has  been  determined. 
Likewise  there  exist  a  3x3  matrix  B  that  transforms  a  vector  in  the  OXYZ 
coordinate  system  into  the  OUVW  coordinate  system: 


P  =  B  p 
Kuvw  xyz 

Since  any  complex  rotation  can  be  divided  into  three  component 
rotations,  the  next  step  in  developing  the  forward  kinematics  is  to 
derive  the  basic  rotation  matrices  for  rotation  about  the  three  axes 
of  the  reference  frame.  Figure  3.3  shows  a  rotation  about  the  OX  axis  by 


Figure  3.2  Body  Attached  Coordinate  Frame  and  Reference  Frame 
Obviously  point  p  can  be  specified  in  either  of  the  coordinate 


systems,  that  is: 


p 

^uvw 

p 

xyz 


^u’pv’pw]t 

(Px’Py’Pz)1 


If  the  block  is  rotated  by  some  arbitrary  angle,  the  point  p,  which 


is  fixed  to  the  block,  is  also  rotated  [Figure  3.3].  That  is,  the  vector 
(px,p^,pz)t  has  changed  while  the  vector  ( , py , ^  has  remained 
constant. 


A  rotation  matrix  (3x3)  can  be  constructed  that  maps  the  co¬ 
ordinates  of  a  position  vector  in  a  rotated  coordinate  system  (OUVW)  in¬ 
to  a  reference  coordinate  system  (OXYZ)  [Lee,  1982].  Therefore  the 

vector  p  =  p  i  +  p  i  +  p  i  ,  when  projected  onto  the  coordinate 
Kuvw  Ku  u  Kv  v  Kw  w’  J 

frame  OXYZ  axes,  will  yield: 


In  order  to  perform  even  the  smallest  movement  of  a  robotic  arm 
(manipulator) ,  there  must  be  specific  changes  in  the  angles  or  displace¬ 
ments  that  exist  between  the  set  of  links  and  joints.  The  manipulator's 
position  and  orientation  in  space  may  be  specified  by  examining  the 
position,  orientation,  and  dimensions  of  each  link. 

The  kinematics  problem  is  generally  divided  into  two  parts: 
forward  kinematics  is  concerned  with  the  position 
and  orientation  of  the  manipulator  given  a  set  of 
joint  angles. 

inverse  kinematics  is  concerned  wi ch  deriving  a 
legitimate  set  of  joint  angles  given  a  position 
and  orientation  of  the  end-effector  of  a  robot  in 
some  reference  coordinate  system. 

Forward  Kinematics 

In  viewing  the  forward  kinematics  problem  it  is  necessary  to  in¬ 
vestigate  the  relationship  between  a  stationary  or  reference  frame  and 
a  coordinate  system  that  is  able  to  revolve  and/or  translate.  Lee  [Lee, 
1982]  discusses  this  topic  using  a  rigid  body  example,  where  there  is  a 
body-attached  coordinate  frame  on  a  block  located  in  a  reference  co¬ 
ordinate  system  [Figure  3.2], 

In  this  figure  we  have  two  right-handed  rectangular  coordinate 
systems.  OUVW  is  a  body-attached  coordinate  system  that  will  change 
position  and  orientation  as  the  rigid-body  does  so.  OXYZ,  on  the  other 
hand,  is  a  fixed  reference  frame.  The  purpose  here  is  to  develop  a  given 
transformation  matrix  from  the  OUVW  coordinate  system  to  the  OXYZ 
coordinate  system. 


CHAPTER  III 


ROBOTIC  MANIPULATOR  KINEMATICS 

In  this  chapter  robot  configurations  and  kinematics  are  reviewed  and 
a  description  is  given  of  the  specific  robot  manipulator  used  in  this 
study  and  of  the  computer  network  providing  control  for  that  manipulator. 
Then  a  description  of  the  system  equations  used  in  the  particular 
estimation  problem  which  illustrates  the  integration  of  the  vision 
acquisition  system  with  the  mechanical  manipulator  arm  is  presented. 

Kinematics  is  basically  a  description  of  the  geometries  associated 
with  a  mechanical  system.  Many  industrial  robots  of  today  consist  of  N+l 
rigid  bodies  called  links  and  N  connections  known  as  joints.  There  are 
two  types  of  joints,  translational  and  revolute,  whose  names  indicate  the 
types  of  motions  the  joints  perform  [Figure  3.1]. 


Figure  3.1  Two  Types  of  Robotic  Joints 


Figure  2.13  Cross  Hairs  Method 
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problem  the  idea  of  coordinate  system  transformation  is  applied  to  a 
robotic  arm.  Because  vhe  links  of  the  robot  can  rotate  and/or  translate 
with  respect  to  a  reference  coordinate  frame,  a  body-attached  coordinate 
system  is  established  at  the  joint  for  each  link  [Lee,  1982]. 

The  establishment  of  these  body-attached  coordinate  frames  were 
initially  described  by  J.  Denavit  and  R.S.  Hartenberg  [Oenavit  and 
Hartenberg,  1955].  Their  method  is  outlined  in  [Paul,  1981]  and  [Lee, 
1982].  The  general  principles  consist  of  three  rules  (i  refers  to  the 
ith  link,  x,y,z  are  coordinate  directions): 

1)  the  z-  axis  lies  along  the  axis  of  motion  of 
the  (i+1 )th  joint. 

2)  the  x .  axis  is  along  the  axis  resulting  from 
the  cross-product  of  the  z .  ^  and  z-  axes, 

‘i 


,  ,  ,  .  I  I  *-  -j  _  1 "  -j  I  i 

or  along  the  common  normal  between  the  z-  and 

z.  i  axes.  The  y^  axis  is  chosen  to  complete  the 

right-handed  coordinate  system. 

t  h 

3)  The  origin  of  the  iL  coordinate  frame  lies  either 
at  the  intersection  of  the  z^  ^  and  z^  axes  or  at 
the  intersection  of  their  common  normal  with  the 


z.  axis. 


Denavit  and  Hartenberg  also  present  four  additional  quantities 
that  completely  describe  any  revolute  or  translational  joint.  It  is 
important  to  recognize  that  the  links  maintain  a  fixed  relationship 
between  the  joints  [Paul,  1931].  The  four  parameters  are: 

.  -  the  joint  angle  from  the  x -  ^  axis  to  the 

x-  axis  about  the  z .  ^  axis 
d^  -  the  distance  from  the  origin  of  the  (i-l)th 
coordinate  frame  to  the  intersection  of  z ■  ^ 
axis  with  the  x^  axis  along  the  z^  ^  axis, 
a-  -  the  offset  distance  from  the  intersection  of 
the  z-  i  axis  with  the  x^  axis  to  the 
origin  of  the  i th  system  along  the  x-  axis  (or 
the  shortest  distance  between  the  z^  ^  and  z^ 
axes ) . 

^  -  the  offset  angle  from  the  z^-j  axis  to  the  z^ 

axis  about  the  x-  axis  (using  the  right-hand 
rule)  [Lee,  1982]. 

Depending  on  the  type  of  joint  some  of  the  quantities  are  constants. 
That  is,  for  a  revolute  joint  the  values  of  d-,  a.  and  remain  constant. 

Since  •  changes,  it  is  given  the  term  "joint  variable."  While  with  a 
translational  joint  the  values  of  a^ ,  a.  and  ^ .  are  constant  and  d^ 
becomes  the  joint  variable  [Figure  3.10]. 

Once  the  coordinate  system  for  each  link  has  been  established,  a 
homogeneous  transformation  matrix  relating  the  coordinate  system  of  link 
i  to  that  of  link  (i-1)  can  be  found.  That  is,  the  orientation  of  the 
i th  coordinate  system  in  the  (i-1 ) t h  system  can  be  reduced  to  four  basic 


transformations : 
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Link  i 


Figure  3.10  Joint  Variables  and  Quantities 

1)  rotation  about  z-_^  axis  an  angle  ^  to  align  the  x  ■  ^ 
axis  with  x^  axis. 

2)  translation  along  the  z  ■  ^  axis  a  distance  so  that 
x.  i  and  x.  become  coincident. 

3)  translation  along  the  x-  axis  a  distance  a.  to 
bring  the  origin  of  both  coordinate  systems  into 
coi ncidence. 

4)  rotation  about  the  /•  axis  an  angle  .<..  so  that 
Zj  j  axis  and  2-  axis  are  aligned. 

As  a  result  the  homogeneous  transformation  matrix  relating  any  ith 
coordinate  system  to  the  (i-l)th  coordinate  system  would  be  the  matrix 
product  of  these  four  basic  transformations: 
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A  =  T  T  T  T 
i -1  z , v  z,d  x  ,a  x,. 


For  revolute  joints,  the  resulting  matrix  takes  the  form 


icos 'i 


-cos^s  in:-.  s i n s i n  ^  a^cos  ■ 


sin:.  cosn .  cos- .  -sin^cos-:-. 


3  i 5 1 n  ■  i 


sin. 


COSa^ 


and  for  translational  joints,  it  becomes 


icos'-'i 


i  s  i n  i 


-cosx -  sin-.- . 


cos ^ cos: . 


sin^-sinv . 


-sinu-cosv • 


sin. 


COSa- 


0' 

0 

d. 


Forward  Kinematic  for  P-5  Robot 


The  robot  used  in  this  thesis  research  was  the  General  Electric  P-5 
process  robot.  A  graphical  depiction  of  the  robot  can  be  found  in  Figure 
3.1.  Using  the  Denavit  and  Hartenburg  method  for  establishing  co¬ 
ordinate  frames,  and  both  Paul  and  Lee's  descriptions  on  how  the  homo¬ 
geneous  matrix  between  joints  and  coordinate  frames  can  be  established, 
the  following  description  of  the  forward  kinematics  of  the  P-5  was 


arrived  at. 
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P'3  *4  5 


y4  z/3*  y5 


*1  dl 


"-^*0  y0 

Figure  3.11  Five  Link  Robot  Similar  to  the  P-5  Process  Robot 
In  Table  3.1  the  necessary  link  parameters  of  the  P-5  process  robot  are 
tabulated. 

Table  3.  1  P-5  Robot  Joint  Parameters 


Joi  nt 


Home 

posi tion 


Constant  parameters 


able 

'i 

a 

X 

d 

1 

0° 

0 

o 

O 

cr> 

65cm 

2 

90° 

60cm 

0° 

0 

3 

-90° 

80cm 

0° 

0 

4 

90° 

0 

90° 

0 

5 

0° 

0 

0° 

10cm 

In  order  to  establish  the  position  of  a  point  in  coordinate  system 
i  referred  to  the  base  coordinate  system  we  must  develop  the  homogeneous 
transformation  matrix  °T^.  This  is  given  by  the  product  of  the  individual 


transformation  matrices 


Mo  A1 


Al-r 
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The  matrix  as  was  stated  in  the  previous  section  is  a  specific 
application  of  the  general  matrix  a|. whose  form  was  given  previously. 
For  the  P-5  robot  with  the  notation  Cos =  C-  and  Sin  •  =  ,  the  in¬ 
dividual  A  matrices  take  the  form: 


,C1  0  S1  0  : 

|s^  0  -C1  0  ,  d^  =  65  cm  a  =  0.0  cm 

;o  1  o  d1  I  =  90° 

|o  o  o  i  _| 


1 C  2  ”S2  0  a2C2  i 

^2  0  a2^2 1  d2  =  0°  a2  =  60  cm 

!  i 

,0  0  1  0  |  x2  =  0° 

o  o  o  i  _! 


• C3  ~S3  0  3C3i 

i 

i S3  0  ^2^3  i  =  0  cm  =  80  cm 

h2  j0  010,  a3  -  0° 

io  ooi  _i 


;s4  0  -C4  °l 

|o  1  0  0 1 

;o  0  0  i\ 


d  =  0  cm  a^  =  0  cm 

,4  -  90° 


iC5  "S5  0  0  i 

I  Sr  C  r  0  0  ( 

1  i 

0  0  1  d5( 

!o  0  0  lj 


d  =  10  cm  a  =  0  cm 
x  =  0° 


Now  the  coordinates  of  any  link  relative  to  some  reference  coordinate 
frame  can  be  expressed  as  an  appropriate  product  of  these  matrices. 


For  example,  if  one  wishes  to  express  some  point  in  the  coordinate 
system  attached  to  link  5  in  terms  of  the  reference  coordinate  system  of 
link  4  we  merely  perform  the  matrix  multiplication  expressed  by: 


4  5 

A3  A4  p5 

1^4  0  $4  0|  1^5  ^5  ^  ^ 

; ^4  0  -C^  0 ;  i S5  ^5  0  0 

'0  1  0  0!  'o  0  1  d, 

1  I  j  5 

!o  00  jl!  |o  0  0  lj 

,SC5  "C4S5  S4  d5S4 ' 
i^4C5  -^4S5  “^4  _ d 5 C 4  j 

I  S5  C5  0  0  ! 

:_o  0  0  1  _j 

Now  suppose  one  wished  to  express  a  point  in  the  coordinate  system 


attached 

to  link  5  in  the 

base  coordinate 

system. 

This  can  be  expressed 

as : 

p  =  °T,-  d r  =  A1 
0  5  5  0 

A?  A2  A3  A4  p5 

where 

°T  2  A^  A^  A4 

5  Mo  A1  2  M3 

A5 

M4 

The  general  transformation 

matrix  takes  the  form 

% 

=jClC234C5+SlS5 

'C1C234S5+S1C5 

C1 S234 

Cl^d5S234+a3C23+a2C2^ 

S 1 C234C5-C 1 S5 

~S1C234S5”C1C5 

s  s 

V234 

Sl^d5S234+a3C23+a2C2-' 

S234C5 

"S234S5 

"C234 

'd5C234+a3S23+a2S3+dl 
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Therefore  with  the  general  matrix  transformation  °T^  any  position  of  the 
end  effector  expressed  in  the  fifth  coordinate  system  can  easily  be 
transformed  to  base  coordinates. 


Inverse  Kinematics 


The  inverse  kinematic's  problem  may  be  the  more  important  problem 
in  this  discussion.  The  reason  for  this  is  simply  that  most  robotic 
arms  or  manipulators  are  positioned  and  oriented  by  a  combination  of 
joint  angles.  This  gives  importance  to  the  ability  to  transform  real 
world  coordinates  into  this  particular  set  of  joint  angles.  "Obtaining  a 
solution  for  the  joint  coordinates  requires  intuition  and  is  the  most 
difficult  problem  we  will  encounter  ..."  [Paul,  1981]. 

Paul  presents  a  general  approach  to  the  solution  along  with  some 
problems  that  may  be  avoided.  He  also  suggests  that  since  the  end 
effector  of  an  n-link  manipulator  has  a  general  transformation  matrix 

n  H0  H1  n-2  n-1 

by  simply  recursively  premultiplying  the  matrix  by  the  inverse  of  a  link 
transformation  the  specifications  for  the  necessary  joint  angles  can  be 
obtained.  To  facilitate  understanding  of  the  technique  it  will  help  to 
introduce  Paul's  discussion  of  Euler  angles  and  their  solution  [Paul,  1981], 
Euler  angles  can  describe  any  possible  orientation  of  the  end 
effector  in  terms  of  a  rotation  t  about  the  z  axis,  then  a  rotation  • 
about  the  new  y  axis  y'  and  finally  a  rotation  >■  about  the  new  z  axis  z". 

As  previously  discussed,  the  orientation  of  the  end  effector  can  be 
expressed  as  a  product  of  3  rotation  matrices  which  becomes: 
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Euler  (;,■;,>)  = 


j cos;cos .cos*  -  sm:stny 
j  sinjsinosi nv  +  cos; si n, 
j  -s  injects* 

!-  o 


-  cos;cos  sin*  -  sin:cos. 

-  sin;cos.sin,  +  cos; cosy 

si  nvsiny 
0 


cos:si nv  0 

sin;sin-  0 

coso  0 

0  X, 

Since  this  should  yield  the  homogeneous  transformation  matrix  that  was 
presented  in  the  previous  discussion,  it  must  be  true  that: 


I 


n 

I  x 


0. 


a  p 

x  Kx 


Euler  = 


y 


y 


y 


'nz  °z  az  pz 


!  0  0  0  1_, 

As  a  result,  the  unknown  Euler  angles  should  be  determinable  from  this 
matrix  equation.  The  obvious  solutions  from  these  equalities  are: 


'  =  cos"1 (az ) 

f  =  cos"1 (ax/sino ) 
y  =  cos’1 (-nz/sin  ;) 

As  Paul  points  out  there  are  several  problems  with  this  approach. 

1)  In  using  the  arc  cosine  function: 

a)  the  sign  is  undefined 

b)  the  accuracy  in  determining  the  angle  itself  is  dependent 
on  the  angle 


53 


2)  In  solving  for  ;  and 

a )  problems  with  the  arc  cosine  as  discussed  above 

b)  use  of  sine  leads  to  inaccuracy  whenever  sin.  !0 

c)  values  undefined  when  :  =  0  or  +  180° 

Paul  suggests  looking  for  arctangent  functions  in  doing  his  iterative 
approach  and  goes  on  to  justify  his  choice  of  arctangents  with  the  follow¬ 
ing  arguments. 

a)  angles  are  returned  in  range  by  taking  into  account 

the  signs  of  the  ordinate  and  abscissa 

b)  ability  exists  to  detect  zero  ordinate  or  abscissa  and 
return  proper  result 

c)  accuracy  for  arctangent  is  uniform  over  full  range  of 
defini tion. 


Inverse  Kinematics  for  the  P5  Robot 

For  the  application  of  this  technique  to  the  P-5  robot  the  °T5  matrix 
can  be  expressed  as: 

°T  =  A^  A^ 

i 5  mq  *2  *3  h4 


Tne  first  step  in  the  application  of  this  technique  to  the  P-5  robot  is 


tnen  to  premultiply  the  TV  matrix  by  (A  ) 


, ,,  1  -  - 1  o 


(A  '  Tr  -  Tr 
o  a  5 


C,n  +S,n 
lx  1  y 


S,n  -C-,n 
lx  1  y 


ClVSl°y 


ciW, 


S,o  -C-, o  S,a  -Cna 
1  x  1  y  1  x  1  y 


C  i  p  +S-,p 
r  x  1  y 


S, p  -C, p 
rx  lKy 


which  should  also  be  equivalent  to: 
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234C5 

-C  S 
t234i5 

S  234 

d5S234+a3C23+d2C 

234C5 

"S234S5 

-C  234 

'd5C234+a3S23+a2S 

S5 

C5 

0 

0 

0 

0 

0 

1 

To  be  equivalent  each  term  of  the  matrix  must  be  equal  to  the  correspondi ng 
term.  By  equating  the  third  elements  of  columm  four,  can  be  found: 

S,PX  -  C,Py  -  0 

Py 

""I  =  t 

v,  =  tan’’  3. 

1  px 

The  next  angle  of  interest  is  By  equating  the  third  elements  of  the 
1st  and  2nd  columns  ^  can  be  found: 


S5  =  Slnx  "  Clny 
C5  =  Sl°x  "  Cl°y 
(S^nx  -  C i n  1 

>  =  tan  - - p — .  Next  can  be  determined  by  equating 

u  n°x  -  c1oyj 

the  1st  and  2nd  elements  of  the  third  columns. 


S234  ~  Clax  +  Slay 


-1  f(-Clax  -  Slay}' 
234  "  tan  1  T  *~\ 


The  solutions  for  2  ’  3  ’ ' 4  can  de  ^ouncl  us1n9  a  trigonometric  technique. 
After  equating  appropriate  terms  the  identities  of  interest  are: 


ClPx  +  ^ 1 py  S234d5  +  C23a3  +  C2a2 
pz  ‘  di  =  -C234d5  +  C23a3  +  S2a2 


By  manipulating  the  equations  and  defining  two  intermediate  values  it 
can  be  shown  that: 
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q  -  C-, pv  +  -  S234d5  C23a3  +  C2a2 


x 

Vdl 


r  -  P7-d1  +  C234d5  =  S23a3  +  S2a2 


2  _  r2  2  .  r  a  .  r2  2 


'23  3 


'2  23  2  3  2  2 


r  S23a3  +  2S2S23a2a3  +  S2a2 


q2  +  r2  =  (C23  +  S23)a3  +  2a9a^(C?C?^  +  S?S?,)  +  a 


*2a3v  2^23  2  23' 


d3  +  2a3 ( C  2C  23  +  S2S23  ^  +  a2 

Now  by  examining  the  middle  term,  it  can  be  shown  that: 


2  2^23  +  S2S23  cos(v„-.-9)  cosv^  C 


23  '2' 


2  2  2  2 
q  +  r  a3  +  d2  +  2a2a3C3 

2  2  2  2 
q  +  r  -  at-at 

r  =  _ _ - _ L 

3  2a2d3 


s3  =  .♦  /  1  -  C‘ 

•  3  =  tan_1(S3/C3) 


yields  2 


q  = 


C 23 a3  +  C2a2  "  C2C3a3  ”  S2S3a3  +  C2a2 


r  = 


S23a3  +  S2A2  ~  S2C3a3  +  C2S3a3  +  S2a2 


q  S2  (”^3^3)  +  ^2  (^3a3  +  <*2) 


^2  ^3a3  +  a2^  +  ^2  ^3a3^ 


2(C2  +  S2) 


values 


Cramer's  rule  is  then  exployed  in  finding  solutions  for  S2  and  C2: 


C3a3  +  a2 


c2  2 
jd3 


(C3a3  +  a2) 


C  ^  ^  ^  "^"3  /-i  S  ^  ^ 

o  j  2  o  o 

c2  2  rr2  2  ,  0  r  _  2-i 

'b3d3  "  LC3d3  2a3L3  2  a2J 

2  0  2 
'a3  "  2a2d3L3  "  d2 

’  (a3  +  2C3a2a3  +  d2} 


q  ^3d3  +  d?l 


r  S3a3 


=  qS^a^  -  r ( <a ^  +  a^) 


S  -  J?2  _  qS3d3  '  r(C3a3  +  a2 ^ 

2  -(a2  +  2C3a2a3+a2) 


"S3d3 


1 C3a3+a2 


=  ~rS3a3  -  q(C3a3  +  a,) 


r  C0  (r'J3A3  +  q^C3a3  +  d2^ 

^  9  _ L  0  p 

-(a3  +  2C3a2a3+a2) 


tan"  (p-) 
l2 


2  +  3  +  4 


4  “  234  ‘  2 


the  vector  of  joint  angles  is  thus  determined. 


Within  the  working  system  there  are  two  added  constraints  that  have 


an  effect  on  the  specific  form  of  the  matrix.  The  first  is  the  task 
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or  maintaining  the  orientation  of  the  fifth  link  parallel  to  the  floor. 
It  can  be  accomplished  by  properly  selecting  the  vectors  a,  o,  and  n, 
which  were  discussed  previously.  The  set  of  equations  that  specify  this 
task  is: 


Calibration  of  Camera  to  Robot 


In  this  project  an  additional  transformation  is  due  to  the  introduction 
of  the  camera  into  the  system.  The  camera's  image  field  acts  as  another 
coordinate  frame.  As  a  result  of  the  restrictions  in  link  5  and  the  fact 
that  the  camera  is  mounted  to  a  fixed  frame  [Figure  3.12],  the  camera  lens 
is  maintained  parallel  to  the  floor.  Therefore,  this  transformation  is 
only  necessary  in  two  dimensions. 


Figure  3.12  P-5  Robot,  Vidicon  Camera ,  Laser 
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deviation  are  relatively  of  the  same  size  in  the  range  .5-1.5  for  the  x 
and  y  coordinate  and  also  the  radial  distance. 

The  next  significant  source  of  error  was  that  due  to  the  repeat¬ 
ability  of  robot  motion.  That  is,  what  is  the  standard  deviation  of  the 
error  between  a  desired  position  and  the  actual  position,  when  the  robot 
is  repeatedly  asked  to  go  to  the  desired  position.  In  this  test  the  robot 
was  started  at  the  home  position.  The  image  of  a  stationary  object  was 
used  to  direct  a  motion  of  the  robot.  When  the  move  was  completed,  a  new 
image  of  the  object  was  taken,  and  the  coordinates  of  its  center  were 
determined.  The  results  of  IOC  such  trials  were: 


-  6.12 


In  each  case  an  average  value  was  determined.  Then,  using  this 
value  as  the  mean,  a  variance  and  standard  deviation  were  calculated.  In 
this  experiment  it  is  obvious  that  the  repeatabi 1 i ty  of  a  move  is  different 
in  the  x  and  y  direction.  Because  these  standard  deviations  are  expressed 
in  pixels  the  true  deviation  of  the  repeated  position  is  rather  small.  How¬ 
ever  these  pixel  values  provide  a  basis  for  evaluating  the  overall  system 
error.  Without  any  estimation  taking  place  these  values  give  an  indication 
of  how  accurately  the  robot  can  be  placed  over  a  stationary  object.  It 
is  obvious  that  in  the  overall  system  the  fact  that  a  moving  object  is 
used  and  an  estimation  is  attempted  should  yield  higher  expected  standard 


deviations. 


The  time  constraint  on  the  minimum  estimation  time  is  influenced 
by  two  specific  times.  One  was  the  time  used  to  calculate  the  object's 
estimated  position,  and  the  second  was  the  time  period  between  the  be- 
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TABLE  4.1  Image  Processing  Error  Data 
1A)  x  =  99.725001  •  -  1.161368  IB)  x  =  99.690002  :  =  1.111713 

A  A 

y  =  84.803001  j  =  .471371  y  -  84.830002  ;  =  .501097 

R  =  130.918182  ,-R  =  .818020  R  =  130.901230  -R  =  .769482 

» 

2B)  x  =  29.976999  :x  =  .877766 

y  =  33.471001  7  =  .571981 

y 

R  -  44.941757  :R  =  .517673 


2A)  x  =  29.895  jv  =  .876342 

A 

y  -  33.395  7  =  .575304 

y 

R  =  44.830269  :R  =  .531333 


3A)  x  =  223.826004  •  =  1.010803  3B)  x  =  223.617996  a  =  .966475 

A  X 

y  =  136.552994  -  .670218  y  =  186.472000  a  =  .575512 

y  J  y 

R  =  291.377655  oR  =  .805518  R  =  291.166077  jR  =  .754707 


X  = 

57.59 

X 

=  .813569 

4B) 

x  =  57.681999 

°x  = 

.908227 

y  = 

187.220001 

;y 

=  .677937 

y  =  187.162994 

°y  = 

.546288 

R  = 

195.878860 
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deviation  from  this  average  value  was  derived.  The  data  derived  in  the 
experiment  is  presented  in  Table  4.1.  The  data  in  this  table  represents 
experiments  performed  at  five  locations  within  a  zoomed  image  field. 

These  five  locations,  labelled  1  through  5  in  the  table,  include  the  four 
corners  and  the  center  of  the  zoomed  image  field  in  order  to  detect  any 
variations  over  the  field.  Each  position  is  represented  by  two  experi¬ 
ments  (A  and  B).  Each  block  of  experimental  results  consist  of  the  average 
object  center  x  coordinate  and  y  coordinate.  The  average  radial  distance 
from  the  image  field  origin,  and  the  corresponding  standard  deviations. 

In  addition,  the  table  includes  a  list  of  the  average  standard  deviations 
over  all  data. 

The  data  in  the  table  reveals  some  interesting  features  about  the 
image  acquisition  system  for  the  static  case.  The  most  obvious  result 
is  that  the  standard  deviation  in  the  y  direction  is  consistently  lower 
then  that  of  the  x  direction.  At  this  time  there  is  no  obvious  reason 
for  this  type  of  bias.  Another  interesting  result  that  is  evident  is 
that  the  standard  deviations  of  the  x  and  y  coordinates  and  radial 
distance  are  on  the  average  less  than  1.0  pixel.  The  data  in  this  experi¬ 
ment  typifies  the  actual  data  received  in  the  working  system.  That  is  the 
zooming  feature  of  the  imaging  system  has  been  incorporated  so  that 
results  of  data  acquisition  within  the  tracking  experiment  would  be  con¬ 
sistent  with  the  data  provided  here.  This  simply  means  that  even  with  the 
reduced  resolution  of  the  object  in  a  zoomed  mode  the  variance  from  the 
mean  would  be  less  then  a  pixel  in  the  static  case.  The  third  interesting 
feature  revealed  by  this  experiment  is  that  no  area  of  the  camera  field 
reveals  any  true  deviation  from  value  of  other  areas.  That  is  the  standard 
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The  dynamics  of  the  P-5  robot  seem  to  be  the  major  source  of  error. 
When  the  robot  is  directed  to  proceed  to  a  position,  although  there  is  a 
form  of  velocity  control,  it  is  obvious  that  overshoot  occurs  and  vibration 
is  present  at  the  conclusion  of  the  move.  This  vibration  may  cause  slight 
sniffing  of  the  camera  or  inisposi tioning  of  the  robot.  Both  of  these 
problems  would  result  in  errors  for  the  image  processing  application.  An¬ 
other  problem  introduced  by  the  P-5  robot  that  may  cause  inaccuracies  in 
the  calculations  or  even  in  the  actual  positioning  is  the  repeatabi 1 i ty  of 
the  robot.  That  is,  given  a  position  command  how  accurately  can  the  robot 
repeat  this  position? 

There  are  additional  considerations  which  must  be  taken  into  account. 
For  example,  throughout  the  research  the  presence  of  noise  in  the  image 
field  has  been  observed.  This  source  of  error  influenced  the  choice  of 
the  center  finding  technique  as  discussed  previously.  Furthermore,  com¬ 
putational  and  mechanical  movement  times  put  limits  on  the  minimum  pre¬ 
diction  time. 

The  first  source  of  error  of  interest  is  that  introduced  by  the 
image  aquisition  system.  As  was  stated  previously,  the  main  task  of  this 
system  is  to  determine  the  value  for  the  center  of  the  object  that  is 
being  tracked.  This  does  not  have  to  be  the  true  centroid,  however 
the  value  that  is  determined  should  be  repeatable  to  some  degree  of 
accuracy  even  in  a  "noisy"  environment.  The  test  to  discover  the  error 
in  this  measurement  was  a  static  test.  Although  the  motor  power  to  the 
robot  was  on,  there  was  no  directed  movement  during  the  test.  The  pro¬ 
cess  consisted  of  placing  an  object  at  several  locations  in  the  image 
field  and  making  1000  measurements  at  each  position.  For  each  position 
an  average  x,y,  and  (x  +  y  )  value  was  determined  and  a  standard 
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EXPERIMENTAL  RESULTS  AND  ANALYSIS 

In  this  chapter  a  number  of  experiments  and  their  results  used  to 
evaluate  the  performance  of  the  "intelligent  robot"  system  are  pre¬ 
sented.  At  the  conclusion  of  the  chapter  a  Kalman  filter  model  of  a 
linear  estimator  is  given  in  order  to  have  a  basis  for  evaluating  the 
experimental  results  obtained. 

In  order  to  facilitate  the  analysis  of  the  data  recorded  in  this 
research,  it  will  be  important  to  understand  in  general  what  the  specific 
sources  of  errors  are.  To  this  end  an  attempt  was  made  to  examine  all 
the  contributions  to  the  overall  error  in  terms  of  camera  pixels. 

Experiments  and  Results 

The  system  as  described  in  the  previous  chapter  contains  a  number  of 
devices  that  may  cause  the  introduction  of  errors  into  the  system. 

Although  the  software  has  tried  to  compensate  for  image  acquisition  errors 
there  are  a  number  of  possibilities  that  may  arise  that  could  raise  the 
probability  of  inaccuracies  in  the  image  gathering  technique.  For 
example,  if  the  robot  is  vibrating  or  moving  in  any  way  while  the  images 
are  being  taken  each  image  will  not  be  taken  at  the  exact  same  location. 
Even  in  the  static  case  the  software  may  not  derive  the  same  center  of  the 
object  each  time  due  to  noise  or  slight  rotation  of  the  object.  The 
improper  illumination  of  the  object  field  may  also  cause  the  center  of  the 
object  to  shift. 
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In  addition  to  this  estimation  routine  for  the  moving  object  there 
is  a  process  in  the  software  that  attempts  to  avoid  possible  indications 
of  false  velocity.  If  the  object  is  stationary  an  error  in  the  center 
finding  routine  may  indicate  a  change  of  position  and  therefore  a  velocity 
that  is  not  truly  present.  As  a  result,  depending  on  the  estimation 
period,  the  robot  may  be  directed  to  an  erroneous  position.  To  avoid  this 
problem  the  software  observes  very  small  changes  in  positions  as  such,  and 
directs  the  robot  to  remain  at  the  present  position  for  a  specific  sampling 
period.  By  storing  the  robot's  previous  position  in  this  process  a  "catch 
up"  maneuver  can  be  made  when  the  object  has  definitely  traversed  a 
specific  distance.  As  a  result  there  may  be  several  sampling  periods 
when  the  object  is  moving  slowly  enough  to  warrant  the  robotic  manipulator 
to  remain  in  place.  However,  even  with  very  slow  movement,  over  a  period 
of  time  a  large  enough  distance  will  have  been  traveled  so  that  the 
manipulation  must  be  moved  just  to  catch  up  to  the  object. 
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one  having  as  the  initial  position,  the  other  having  x^  as  the  initial 
position.  In  a  similar  manner  the  estimated  position  y^  is  found.  The 
equations  are: 

x3  ~  \  ^X1  +  +  x2  +  x-pT) 

y3  =  7  ^yl  +  2yrT  +  y2  +  yliT^ 

Then  error  terms  in  x  and  y  are  formed  using  the  actual  x^  and  y^ 
values  obtained  from  the  third  image: 


Finally,  the  estimates  x^  and  y^  are  found  as  a  combination  of  the  average 
estimates  resulting  from  using  the  coordinate  positions  of  the  three 
images  as  separate  initial  conditions  and  appropriate  velocities  and  times 
plus  a  contribution  proportional  to  the  error  term.  The  necessary 
equations  are 


+  (x^+x2)tT  + 


xa,gT  +  x2  +  VT  +  xavgT  +  x3  +  xavgT) 


*  xerrT/-T 
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+  yerr^^‘ 

The  predicted  position  x^  at  T  seconds  after  acquisition  of  image 

three  is  the  sum  of  four  terms.  The  first  term  is  the  x^  coordinate 

estimate  using  x-|  as  the  initial  position  and  using  velocities  x-j  and  k^ 

and  their  respective  cT's  and  velocity  x-,  +  x0/Z  =  £  and  time  T.  The 

I  C  a  v  y 

second  and  third  terms  are  similar  to  the  first  but  with  initial  positions 
x^  and  x^  and  using  velocities  x^  anc*  xavg‘  These  three  terms  are  averaged 
and  the  fourth  term  represents  an  error  that  is  linear  with  time.  The  y 
equation  follows  in  a  similar  manner. 
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several  object  images  whose  temporal  relationships  are  know.  The  in¬ 
formation  gained  through  this  sequence  of  images  allows  estimation  of 
object  position  at  a  future  time.  The  limits  on  the  specific  estimation 
time  period  are  governed  by  two  considerations.  First,  the  time  required 
for  image  acquisition,  image  processing,  estimate  calculation,  coordinate 
transformations,  and  response  time  of  the  actual  manipulator  movement 
determine  the  minimum  estimation  time.  Secondly,  the  accuracy  of  the 
estimated  position,  which  decreases  as  the  estimation  period  increases, 
and  the  size  of  the  camera  image  field,  which  limits  the  maximum 
position  error  allowed,  determine  the  maximum  estimation  time  that  will 
insure  that  the  object  is  in  the  field  of  view. 

The  demonstration  example  which  is  used  to  illustrate  the  ideas 
of  this  thesis  consists  of  estimating  the  position  of  a  moving  object 
based  on  information  from  a  sequence  of  three  images.  Figure  3.14 
shows  the  space  and  time  relationships  in  the  estimation  technique  used. 
This  is  also  performed  as  a  software  function.  That  is,  as  the  program 
proceeds,  the  encoder  counters  are  compared  to  the  desired  values.  This 
set  of  differences  is  used  to  determine  the  velocities  of  the  correspond¬ 
ing  links.  The  method  sets  a  joint's  velocity  at  a  fraction  of  the 
maximum  velocity  equal  to  the  ratio  of  its  encoder  count  difference 
to  the  maximum  count  difference  over  all  five  joints.  This  approach  is 
applied  repetitively  as  the  move  progresses,  thereby  providing  closed- 
loop  control . 

The  equations  used  attempt  to  take  into  account  all  information  at 
each  step  for  the  purpose  of  minimizing  large  errors  in  any  one 
measurement.  The  technique  initially  takes  three  images  cT  apart  in 
time  and  finds  their  centers  (x^ ,y-| )  ,(x£,y 


^),  and  (x-^yg).  On  the  basis 
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two  control  loops.  The  first  is  a  tachometer-control  led  loop  which 
produces  a  so-called  "desired"  motor  current.  This  signal  is  compared 
then  to  the  actual  motor  current.  The  error  signal  in  this  loop  will 
determine  the  pulse  width  of  a  20  KHz  pulse  train  used  to  drive  the 
individual  motors  of  the  robot.  The  purpose  of  using  two  control  loops 
is  to  account  for  the  nonlinearities  of  the  robot's  motors. 

The  P-5  process  robot  is  electrically  driven  and  is  capable  of  per¬ 
forming  a  wide  variety  of  industrial  tasks.  It  is  a  five  joint,  five  link 
robot  which  was  initially  designed  for  teaching  playback  type  of  operation. 

The  Estimation  Used  in  Demonstration  Task 

Recall  that  in  this  thesis  the  main  objective  is  to  move  the  robotic 
manipulator  in  response  to  some  visual  information  obtained  from  an  image 
acquisition  system.  This  visual  information  may  be  related  to  an  object 
which  is  either  fixed  or  moving.  The  ability  to  extract  information  such 
as  object  position,  shape,  size,  speed,  direction  of  movement,  or  even 
acceleration,  makes  possible  the  handling  of  a  number  of  industrially  re¬ 
lated  tasks.  Typical  tasks  include  object  removal  from  or  placement  on 
a  moving  conveyor  belt;  painting,  welding  or  other  operations  on  a 
stationary  or  moving  object;  separation  of  objects  based  on  the  results  of 
a  visual  inspection  performed  by  the  image  acquisition  and  processing 
system.  Since  a  number  of  industrial  tasks  require  some  interaction  with 
moving  objects,  the  ability  to  track  or  estimate  positions  of  these 
objects  is  an  important  element  in  the  operating  features  of  an  intelligent 
robot.  These  considerations  have  led  to  the  selection  of  moving  object 
interception  as  the  demonstration  example  used  to  illustrate  the  ideas  in¬ 
vestigated  in  this  thesis.  This  interception  problem  requires  the  use  of 
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robot  angles,  it  converts  these  to  desired  counter  values  and  drives  the 
appropriate  robot  motors  in  order  to  reduce  the  difference  between  the 
desired  and  actual  counter  values.  As  the  difference  becomes  close  to 
zero  proper  robot  link  orientation  will  result  so  that  the  robot  end 
effector  is  in  the  desired  position. 

The  assembler  language  program  running  on  the  CAMAC  performs  two 
basic  functions.  The  first  is  a  command  function  that  interprets  the 
information  from  the  serial  line  as  data  and  commands  to  be  accomplished. 
For  example,  maximum  velocity  can  be  changed,  the  tolerance  that  specifies 
the  completion  of  a  move  can  be  changed,  zeroing  the  values  of  specific 
encoder  counters  can  be  accomplished  and  so  on.  The  second  function  is 
that  of  control.  In  this  loop  the  angles  specified  in  the  new  "move" 
command  are  converted  to  encoder  counts. 

The  velocity  of  the  individual  motors  on  the  robot  can  be  updated 
within  each  loop.  This  allows  for  motion  of  the  manipulator  that  seems 
to  imitate  human  motion.  This  is  also  performed  as  a  software  function. 
That  is,  as  the  program  proceeds,  the  encoder  counters  are  compared  to 
the  desired  values.  This  set  of  differences  is  used  to  determine  the 
velocities  of  the  corresponding  links.  The  method  sets  a  joint's 
velocity  at  a  fraction  of  the  maximum  velocity  equal  to  the  ratio  of  its 
encoder  count  difference  to  the  maximum  count  difference  over  all  five 
joints.  This  approach  is  applied  repetitively  as  the  move  progresses 
thereby  providing  closed-loop  control.  This  assures  that  each  link 
arrives  at  its  designated  position  at  approximately  the  same  time. 

The  output  of  the  CAMAC  is  an  analog  signal  that  has  been  generated 
by  the  CAMAC's  Oigi tal -to-Analog  converters.  This  voltage  is  applied  to 
the  control  box  of  the  P-5  robot.  The  voltage  is  amplified  and  fed  into 


The  system  is  driven  by  means  of  a  high-level  Fortran  program  run  on  a 
PDP  11/60  digital  computer.  The  program  directs  the  flow  of  control 
from  image  acquisition,  through  image  processing,  to  data  delivery  to  a 
CAMAC  computer. 

Following  the  CAMAC  computer  an  interface  has  been  built  to  work 
with  the  control  box  of  a  P-5  process  robot.  The  interface  and  control 
consist  of  several  pieces  of  hardware  that  provide  the  signals  to 
operate  the  motors  of  each  of  the  links  on  the  robot. 

Image  acquisition  is  accomplished  through  an  imaging  system  develop¬ 
ed  by  Imaging  Technology,  Inc.  The  system,  which  was  disucssed  in  greater 
detail  in  Chapter  II,  allows  for  the  acquisition  or  construction  of  2-D 
images.  The  system  provides  the  ability  to  drive  the  imaging  hardware  by 
use  of  high-level  languages,  such  as  Fortran  and  Basic.  There  are  a 
number  of  useful  subroutines  that  allow  for  a  wide  variety  of  processing 
appl ications. 

The  PDP  11/60  is  a  digital  computer  that  acts  as  the  main  controller 
in  the  overall  system.  The  PDP  11/60  processes  the  acquired  data  and 
presents  the  necessary  information  over  a  serial  line  to  the  CAMAC 
computer. 

The  CAMAC  computer  is  basically  a  digital  computer  that  acts  as  an 
interface  between  the  digital  world  of  the  PDP  11/60  and  the  analog  world 
of  the  P-5  robot's  control  box. 

The  CAMAC  operation  monitors  the  values  held  by  a  group  of  up-down 
counters  which  record  the  number  of  encoder  pulses  from  the  corresponding 
joint  angle  encoder  of  the  robot.  The  values  of  these  counters  are  a 
measure  of  the  appropriate  joint  angles  of  the  robot  and  are  zero  at  the 
robots  "home"  position.  When  the  CAMAC  computer  receives  a  set  of  desired 


59 


the  position  of  the  laser.  Step  four  consisted  of  moving  the  robot  to 
several  known  positions  and  determining  the  location  of  the  object  in 
camera  coordinates.  After  each  step  the  vector  that  expresses  the 
position  of  the  end  effector  in  the  coordinates  of  the  fifth  link  is 
derived  using  the  equation: 

p5  -  p0 

As  a  result  a  relationship  between  the  camera  coordinates  and  the  co¬ 
ordinates  of  the  fifth  link  can  be  found.  Finally,  using  a  least 

squares  approach,  the  A.  matrix  transformation  is  determined  [see 

c  dm 

Appendix  A] . 

Components  of  the  System 

A  description  of  the  specific  hardware  and  software  system  under 
consideration  in  this  thesis  is  now  given.  The  system  block  diagram  is 
presented  in  Figure  3.13. 
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Figure  3.13  Block  Diagram  of  System 


The  system  observes  the  location  of  an  object  in  the  image  field  or 


camera  coordinates  p  .  A  transformation  is  necessary  to  express  the 

cam 

object's  position  in  link  5  coordinates  (^cam) •  Obviously,  then  the 
position  expressed  in  base  coordinates  can  be  given  as: 

p  =  T°  A5  p 

Ko  5  cam  pcam 

5 

The  transformation  matrix  A  simply  represents  a  set  of  equations 

cam 

that  take  the  form  of: 

x5  =  axcam  +  bycam  +  xo 

^5  cxcam  +  b-Cam  +  o 


These  equations  can  be  expressed  in  a  homogeneous  matrix  form  as: 
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The  system  used  in  this  research  did  not  have  a  permanently  attached 
camera.  As  a  result,  a  calibration  of  the  camera  referred  to  link  5  co¬ 
ordinates  had  to  be  accomplished  whenever  the  camera  was  remounted.  The 
basic  purpose  of  the  calibration  was  to  derive  the  six  unknown  constants 

5 

of  the  A„om  matrix, 
cam 

The  logical  design  of  the  calibration  routine  was  relatively 
straight  forward.  Step  one  consisted  of  positioning  the  robot  in  a 
'home'  orientation.  Next,  a  laser  which  is  mounted  along  with  the 
camera  was  used  to  produce  a  light  image  in  the  camera  field.  Because 
the  laser's  position  is  fixed  in  reference  to  the  camera's  position,  the 
location  of  the  light  image  in  the  camera  field  is  also  fixed.  Step 
three  was  to  move  the  robot  to  a  known  position  and  place  an  ob.  net.  under 


ginning  of  a  move  and  the  completion  of  the  move.  These  two  values  were 
examined  by  using  a  system  routine  that  was  able  to  specify  the  time 
increments  to  within  1/60  second.  The  necessary  parts  of  the  program 
were  repeated  and  timed  as  was  the  actual  movement  of  the  robot.  The 
experiment  was  conducted  using  a  wide  range  of  robot  motion.  After 

running  through  the  procedures  1000  times  the  values  obtained  were: 

AVERAGE  TIME  FOR  CALCULATION  =  .291  seconds 

AVERAGE  TIME  FOR  ROBOT  MOVE  =  .854  seconds 
The  data  in  this  section  is  provided  to  indicate  to  the  user  the 
average  amount  of  time  needed  to  process  the  data  in  software,  derive  an 
estimate  of  the  object's  position,  and  to  actually  move  the  robotic  mani¬ 
pulator.  The  total  of  these  two  values  will  yield  the  minimum  estimation 
time  that  the  system  can  work  with.  Using  a  smaller  estimation  time 
would  result  in  the  robot's  inability  to  beat  the  object  to  the  position 
even  if  all  error  was  removed  from  the  system. 

In  the  following  section  an  evaluation  of  the  overall  system 
accuracy  expressed  in  pixels  is  given.  The  experiment  for  obtaining  data 
to  examine  the  general  system  consisted  of  running  the  system  with  a  minor 
software  modification.  This  modification  was  the  addition  of  a  timing 
loop  that  timed  out  the  estimation  time  using  the  system  clock.  At  this 
point  an  image  was  taken  and  the  object's  position  was  examined 

The  experiment  consisted  of  running  this  modified  program  through  a 
loop  100  times.  In  the  loop  the  robot  started  from  the  home  position 
each  time.  At  this  location  the  acquisition  of  three  images  of  a  moving 
object  was  accomplished.  Using  these  images,  an  estimate  of  the  object's 
position  at  a  specific  time  later  was  determined.  The  manipulator  was 
directed  to  move  to  this  estimated  position  where  it  waited  for  the 
system  timer  to  indicate  that  the  estimation  period  had  elapsed.  An 
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image  was  taken  and  evaluated  to  determine  the  object's  new  position 
compared  to  the  location  of  the  laser  in  the  image  field  during  cali¬ 
bration.  It  is  assumed  that  the  laser  position  can  be  considered  the 
system  pointer  because  the  initial  calibration  of  the  system  was  per¬ 
formed  using  the  laser  image  location  as  a  reference  point.  An  error, 
expressed  in  pixels,  in  both  the  x  and  y  directions  was  calculated  and 
stored.  Finally,  the  robot  was  directed  back  to  its  home  position  and 
the  process  began  again. 

After  the  sequence  of  loops  the  error  information  was  used  to  de¬ 
termine  an  average  error  and  a  standard  deviation.  The  experiment 
examined  both  linear  and  circular  motion  using  estimation  times  of  three, 
five,  seven  and  nine  seconds. 

The  different  types  of  motion  were  achieved  by  using  a  small  DC 
motor.  Linear  motion  was  established  by  connecting  the  object  to  the 
driveshaft  of  the  motor  by  means  of  a  piece  of  thread.  The  driveshaft 
was  driven  at  a  constant  rpm  and  as  a  result  the  thread  was  reeled  in  at 
a  constant  rate.  The  system  was  oriented  so  the  object  would  move  in  a 
purely  x  direction  or  a  purely  y  direction.  The  velocity  of  the  object 
was  measured  at  36  pixels/sec.  Circular  motion  was  provided  by  placing 
a  piece  of  cardboard  with  the  object  attached  onto  the  driveshaft  of  the 
motor.  The  object  was  located  a  distance  of  4  inches  from  the  shaft  and 
had  an  angular  velocity  of  1.5  rpm.  In  Table  4.2  the  data  for  this  experi¬ 
ment  i s  1 isted. 

The  information  in  Table  4.2  consists  of  two  sets  of  data 
associated  with  linear  and  circular  object  motion.  The  data  consists  of 
four  values,  the  average  error  in  both  the  x  and  y  direction  along  with 
the  appropriate  standard  deviations.  The  error  value  were  derived  by  ex- 


Table  4.2  Overall  System  Error  Test  Data 


a)  Linear  Motion 


pure  y  direction 


pure  x  direction 

II 

II 


b)  Circular  Motion 

rc 


Estimation 
T  ime 


(sec. ) 

X 

e 

“X 

y 

3 

1 .84 

13.89 

1.5474 

4.5098 

5 

1.83 

23.41 

1.2966 

4.5455 

7 

2.49 

31.41 

2.1748 

6.6485 

9 

3.74 

55.92 

2.4067 

10.6035 

3 

15.54 

4,27 

2.3934 

3.2276 

5 

24.73 

4.99 

2.8770 

3.4569 

7 

28.96 

6.17 

4.8988 

4.1954 

9 

40.88 

10.60 

6.1307 

7.6766 

3 

53.8 

62.34 

28.4109 

31.1414 

5 

79.82 

91.13 

38.7372 

45.4492 

7 

103.27 

117.55 

47.6411 

55.3914 

9 

153.78 

160.68 

70.8511 

80.1495 

amining  the  position  of  the  object  at  the  estimation  point  in  reference 
to  the  laser's  position  in  the  image  field.  It  is  interesting  to  note  the 
shift  in  the  magnitude  of  error  depending  on  whether  the  linear  motion  is 
in  the  x  or  y  direction.  The  inactive  direction  has  a  smaller  value  of 
error  compared  to  the  active  direction.  This  small  error  can  come  about 
from  inaccuracies  in  determining  the  object's  centers  and  may  grow  with 
the  time  of  estimation  if  consecutive  errors  imply  a  velocity  which  is  in 
fact  not  present.  However,  as  the  data  shows,  for  reasonable  prediction 
times  these  errors  can  be  considered  negligible.  As  a  result,  important 
information  can  only  be  discerned  with  the  active  direction  data. 

Model  for  Experimental  Results 

A  framework  for  interpreting  the  experimental  data  presented  in  the 
previous  section  is  developed  in  the  following  paragraphs.  Figure  4.1 
graphically  shows  how  the  error  variance  of  a  linear  predictive  motion 
model  changes  with  prediction  time.  The  simulation  is  performed  using 
a  discrete  linear  Kalman  filter  expressed  by  the  equations  given  in 
Table  4.3.  This  model  was  used  because  the  Kalman  filter  is  considered 
the  best  (minimum  error  variance)  linear  filter  for  either  state  observa¬ 
tion  or  estimation  [Sage  and  Melsa,  1971].  As  a  result,  by  comparing  how 
the  error  variance  would  change  with  time  in  the  experiment  compared  to 
the  values  given  in  the  Kalman  filter,  an  indication  of  relative  accuracy 
for  the  overall  system  is  obtained. 

Figure  4.1  shows  how  the  error  variance  increases  with  increasing 


estimation  time.  This  graph  was  determined  by  examining  an  equation  that 
yields  the  change  of  error  variance  through  a  linear  system  [Sage  and  Melsa, 
1971].  The  equation  is: 
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Table  4.3  General  Discrete  Kalman  Filter 


Message  model 

x ( j + 1 )  =  ;(j+l,j)x(j)  +  : ( j )w( j )  +  B(j) 

u(j) 

Observation  model 

z(j)  =  H( j )x( j )  +  v(j)  +  y( j ) 

Prior  statistics 

E-,w( j ) >  =  m  (j)  Etv(j)}  =  -  (j) 

E i x ( 0 ) }  =  c  (0) 

C0V’_w(j)  ,w(k) }  =  Vw ( j ) 0 K ( j  -  k) 
cov- v(j)  ,v(k)>  =  Vv( j )oK( j  -  k) 
cov.w(f) ,v(k)}  -  ywv ( j ) i K ( j  -  k) 


var;x(0)l  =  Vx (0) 

One-state- 
prediction 
algori thm 

x(j+l;j)  =  t(j+l,j)x(j)  +  :(j)  w(j)  +  B(j)u(j) 

+  Kp( j )[z( j )  -  cv(j)  -  y(j)  -  H ( j ) x ( j ) ] 

Filter  algorithm 

x(j+l)  =  x(j+l  j)  +  K(j+1 )[z(j+l )  -  uv(j+l)  -  y(j+l) 

-  H( j+1 )x(j+l 1 j)] 

One-state- 
prediction  gain 
algori thm 

Kp(j)  *  r(j)vwv(j)Vw-'(j) 

Fi 1  ter  gain 
algori thm 

K( j+1 )  =  V~(j+1 | j)HT(j+l )[H(j+l )V~(j+l j j)HT( j+1 ) 

+  V/j+l)]"1 

A  priori  variance 
algori thm 

Vx(  j+1  1  j)  =  0(j  +  l,j)  -  Kp(  j  )H(  j  )]V~(  j ) 

xC t( j+1 , j )  -  Kp( j )H( j )]T  +  :  ( j ) Vw( j ) rT( j ) 

-  Kp( j )Vv( j )KpT( j  ) 

A  posteriori 
variance 
algori thm 

V~( j+1 )  =  [I  -  K( j+1 )H( j+1 )]V~( j+1 ! i ) 

Initial 

conditions 

x(0)  =  x (0  0)  =  ux(0)  =  6 { x ( 0 ) } 

Vx(0)  =  Vx(0|0)  =  vartx(O)}  =  Vx( 0) 
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vx(k+l)  -  :(k+l)  Vx(k);T(k+1)  +  :-(k)  Vw(k):T(k) 
where  V  (k)  is  state  error  variance,  ;(k)  is  state  transition  matrix  at 

A 

time  k,  :(k)  is  a  weighting  matrix  and  V  (k)  is  the  variance  of  the  noise 
The  second  term  of  this  sum  is  concerned  with  how  the  noise  on  the  state 
measurement  changes  with  time.  Since  in  the  estimation  technique  the 
prediction  of  an  object's  position  involves  no  measurements  this  term  is 
zero.  Therefore  the  equation  reduces  to: 

Vx(k+1)  =  : ( k+1 )  Vx(kbVl) 

This  equation  reveals  that  the  change  of  variance  with  time  in  the 
estimation  application  is  dependent  on  the  state  transition  matrix  and 
the  previous  variance. 

In  evaluating  the  system  data  in  reference  to  the  Kalman  filter. 
Figure  4.1  shows  that  system  error  variance  in  the  prediction  range  from 
five  seconds  to  eight  seconds  is  very  close  to  that  of  the  Kalman  filter. 
For  both  cases  the  values  of  error  variance  for  estimation  time  above 
eight  seconds  are  too  large  for  practical  prediction.  It  is  important  to 
recognize  that  for  the  range  of  estimate  times  from  three  to  five 
seconds  the  error  variance  of  the  system  is  relatively  constant  and  then 
begins  to  increase  in  the  same  way  as  the  filter  error  variance. 

The  results  here  seem  to  suggest  that  with  regard  to  linear  motion 
estimation  the  variance  changes  of  the  demonstration  system  are  very 
similar  to  those  of  the  Kalman  filter. 

The  circular  motion  experiment  was  conducted  to  examine  how  the 
linear  models  of  the  system  and  Kalman  filter  could  handle  such  a  tra¬ 
jectory.  There  would  definitely  be  a  difference  between  the  Kalman 
filters  and  the  system  in  merely  tracking  the  object  without  estimation. 
Recall  that  there  is  a  constraint  of  minimum  prediction  time  due  to 
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calculation  and  mechanical  movement  of  the  robotic  system.  Therefore  this 
particular  "intelligent"  robotic  system  is  limited  to  using  a  pre¬ 
diction  algorithm.  That  is,  a  need  for  predicting  ahead  at  least  the 
necessary  mechanical  time  is  required.  On  the  other  hand,  the  software 
simulation  does  not  have  this  limitation.  Figure  4.2  shows  that  the 
Kalman  filter  was  able  to  lock  on  to  the  actual  trajectory  after  only  a 
few  iterations.  This  trajectory  was  provided  by  actual  data  of  circular 
object  motion  obtained  by  visually  sampling  the  object  at  .25  second 
intervals. 

Both  the  system  and  filter  faltered  when  attempt  at  prediction  was 
taken.  The  data  for  the  system  reveals  an  average  error  of  more  then  the 
diameter  of  an  object  even  with  a  3  second  prediction  time.  Figure  4.3 
reveals  the  inherent  problem  of  using  a  linear  model  to  predict  circular 
motion.  The  model  uses  the  velocity  of  the  last  data  point  and  predicts 
ahead  based  on  this  trajectory.  It  is  obvious  that  immediately  the 
observations  diverge  from  the  estimations.  Notice  that  at  a  specific 
point  the  errors  start  to  decrease  because  the  circular  motion  is  in  the 
same  direction  as  the  linear  prediction. 
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Figure  4.2  Kalman  Filter  Estimates  and  Experimental  Observations 


CHAPTER  V 


CONCLUSIONS  AND  FUTURE  RESEARCH 

In  this  thesis  a  system  that  can  direct  the  motion  of  an  industrial 
robot  in  response  to  visual  information  acquired  through  an  image 
acquisition  system  was  investigated.  In  this  chapter  conclusions  of  both 
a  general  and  specific  nature  are  given  and  suggestions  are  made  for 
future  research  related  to  this  system. 

The  principal  conclusion  from  this  research  is  that  an  industrial 
robot  can  be  directed  by  information  acquired  from  a  video  source.  The 
system  was  used  to  implement  the  software  discussed  in  this  thesis.  As 
evidenced  by  the  data  presented  in  Chapter  IV,  the  robot  was  able  to 
respond  very  well  to  the  movement  of  an  object  in  the  camera's  image 
field.  Along  these  lines  there  are  a  number  of  constraints  on  the 
system  that  effect  how  it  can  be  used.  As  mentioned  before,  the  time  it 
takes  to  calculate  the  position  of  an  object  in  the  image  field  and  trans¬ 
form  this  data  into  robot  coordinates  takes  approximately  0.2  seconds. 

In  addition  the  average  time  the  robot  takes  to  complete  a  movement  is 
approximately  1.0  second.  Therefore,  even  if  the  system  was  designed  to 
have  an  accurate  object  following  routine,  it  would  be  necessary  to  make 
an  estimate  of  the  object's  position  at  least  1.2  seconds  in  advance.  If 
the  intent  was  to  have  the  system  follow  a  moving  object  with  no  attempt 
made  to  compensate  for  this  time  lag,  system  inaccuracy  would  increase 
with  object  velocity.  Another  interesting  consideration  is  the  basic 
limitation  on  object  velocity.  The  velocity  of  the  object  must  be  limited 
to  that  of  the  robot  in  any  tracking  operation.  Also  since  the  data  for 


82 


83 


linear  prediction  in  Chapter  4  represented  objects  moving  rather  slowly 
(36  pixel/sec)  the  error  values  indicate  that  prediction  times  of  more 
than  7  seconds  cannot  be  justified. 

The  robotic  system  as  it  is  set  up  now  is  unable  to  respond  accurate¬ 
ly  to  movement  of  the  object  while  the  robot  arm  is  in  motion.  This  is 
a  very  complicated  problem  for  both  the  image  processing  technique  and 
the  robotic  controller.  The  acquisition  of  the  image  while  the  robotic 
arm  is  in  motion  may  cause  blurring  which  can  introduce  an  inaccuracy 
into  the  system  that  the  stationary  system  does  not  have.  In  addition 
the  inability  to  know  exactly  where  the  robot  was  at  the  time  when  the 
image  was  taken  yields  the  fundamental  weakness  of  this  dynamic  system  for 
very  accurate  tracking  and  prediction  attempts. 

The  robot  system  controller  as  it  now  operates  allows  for  a  large 
amount  of  overshoot  and  settling  time  at  the  end  of  each  movement.  This 
could  be  a  problem  if  image  acquisition  were  to  take  place  during  this 
settling  time.  The  effect  of  vibration  could  enter  a  larger  amount  of 
inaccuracy  into  any  images  ^aken  during  the  period.  In  addition  the 
system  assumes  that  the  camera  is  parallel  to  the  working  plane  and  re¬ 
mains  at  a  constant  height.  Any  deviation  from  these  constraints  will 
also  introduce  inaccuracies  into  the  system. 

Suggestion  for  Future  Research 

The  system  as  it  now  stands,  and  as  it  was  initially  drawn  out,  is  an 
introduction  to  this  lively  field  of  robotic  vision.  At  this  point 
there  are  a  number  of  areas  where  improvements  are  both  necessary  and 
possible. 
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The  first  area  is  that  or  the  image  processing.  The  fact  that 
noise  is  introduced  by  the  robot's  motors  eliminated  the  use  of  the 
turtle  routine  as  mentioned  in  Chapter  two.  With  the  incorporation  of 
a  turtle  routine  the  ability  to  find  the  true  centroid  of  an  object  even 
a  non -symmetrical  object  would  be  possible.  There  are  a  number  of  methods 
by  which  the  noise  problem  could  be  approached.  Through  simple  software 
techniques  the  system  could  incorporate  a  low  pass  filter  or  average  over 
a  specified  window  within  the  image  field.  The  effect  can  be  seen  in 
Figure  5.1.  The  window  would  pass  over  the  2-D  image  contained  in  the 
frame  buffer  and  examine  a  number  of  pixels  under  the  window  [3x3,  5x5, 

7x7,  etc].  The  algorithm  would  average  all  pixels  and  replace  the  center 
pixel  of  the  window  with  this  average.  Of  course,  if  a  binary  image  was 
being  used  some  threshold  average  would  be  decided  on  and  if  the  average 
was  below  the  threshold  the  center  pixel  would  be  replaced  with  a  black 
pixel,  otherwise  a  pure  white  pixel  would  take  its  place.  The  obvious 
effects  would  be  random  noise  pixels  should  be  eliminated,  the  time 
needed  to  process  the  image  would  be  increased  and  the  image  of  the 
object  would  suffer  a  slight  distortion.  One  must  not  overlook  the 
possibility  of  using  a  hardware  filter  to  eliminate  this  high  frequency 
noise.  Maybe  by  investigating  the  source  of  this  sensitivity  to  noise 
an  appropriate  shielding  technique  can  be  used  to  avoid  the  two  previous 
techniques.  In  either  case  the  tradeoffs  would  need  to  be  examined  and 
weighed. 

The  striking  effect  of  eliminating  the  noise  pixels  and  then  the  in¬ 
corporation  of  the  turfe  boundary/fol  lower  routine  would  be  the  entrance 
into  the  area  of  pattern  recognition  and  the  applications  this  arena  offer. 
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By  delineating  between  shapes  [Dubois,  1984]  the  ability  to  approach  the 
bin-picking  problem  is  facilitated.  At  this  point  the  system  moves 
from  the  periphery  of  the  robotic  vision  research  into  areas  of  current 
research. 

Also  in  the  area  of  image  processing  the  ability  to  move  beyond  the 
area  of  two  dimensional  information  into  the  3-D  arena  is  within  the 
grasp  of  the  system  as  it  now  stands.  The  structure  of  the  laser  source 
mounted  with  the  camera  had  a  two-fold  purpose.  First,  the  laser  pro¬ 
vided  a  reference  point  in  the  calibration  of  camera  to  robot  co¬ 
ordinates.  Secondly  it  offers  the  possibility  of  gauging  depth  between 
the  camera  height  and  working  level.  At  the  present  time  the  system 
incorporates  a  flat  top  table  as  the  working  surface.  However,  by 
incorporating  knowledge  about  the  position  of  the  laser  in  the  image 
field,  the  ability  of  maintaining  a  fixed  height  above  a  variable 
depth  surface  is  not  far  out  of  reach.  In  Figure  5.2  the  effect  of 
driving  the  robot  so  that  the  laser  image  remains  fixed  in  the  image 
field  results  in  this  fixed  depth  application.  The  result  here  may  be 
seen  as  the  ability  to  apply  equal  layers  of  paint  on  a  non-flat  surface 
or  to  inspect  equal  size  areas  of  such  a  surface.  The  true  results  of  such 
an  ability  may  only  be  reaped  when  the  algorithm  is  incorporated. 

In  the  atea  of  kinematics  the  present  movement  is  obviously  awkard. 
That  is,  the  robot  is  directed  to  a  position  and  the  system  then  waits 
for  the  movement  to  be  complete.  At  which  point  the  necessary  information 
is  available  to  give  the  next  direction.  The  system  does  have  the 
capability  of  receiving  a  second  movement  directive  during  the  actual 
initial  move  and  responding  to  it.  However,  by  incorporating  the  mounted 
camera's  visual  information  a  reference  is  needed  to  know  when  and  where 
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Figure  5.2  Image  Field  at  Two  Different  Heights 
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the  visual  information  was  obtained.  That  is  the  °T^  matrix  must 
constantly  be  available  in  order  to  have  the  proper  information  for  an 
updated  movement.  The  most  obvious  result  of  this  improvement  would  be 
a  more  efficient  use  of  the  time  during  which  the  robot  is  moving.  There 
is  a  possibility  that  with  the  acquisition  of  images  during  the  movement 
of  the  manipulator  a  blurring  effect  may  be  introduced  and  as  a  result 
additional  picture  processing  may  be  called  for.  The  field  of  Image 
Enhancement  and  techniques  for  deblurring  of  images  are  some  of  the 
current  topics  in  image  processing. 

The  ability  to  improve  upon  the  technique  for  estimation  in  the 
system's  tracker  application  is  definitely  within  reach.  The  inability  of 
this  system  to  give  effective  estimates  for  circular  motion  could  obviously 
be  improved  by  basing  the  estimate  on  a  curvilinear  model  or  increasing  the 
order  of  the  estimation  equation.  Furthermore  by  i ncorporating  a  type 
of  adaptive  model,  there  is  a  possibility  of  enhancing  the  system’s  per¬ 
formance  with  both  linear  and  circular  motion. 

This  adaptive  solution  may  examine  the  error  values  for  both 
types  of  model  in  an  iterative  fashion.  Depending  on  which  model  yielded 
the  smallest  error  an  estimation  would  be  made.  The  ability  to  improve 
upon  either  estimation  technique  would  result  in  a  tracking  system  that 
could  be  used  in  a  range  of  expectant  trajectories . 
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APPENDIX  A:  LEAST  SQUARES  CALIBRATION  METHOD  AND  SOFTWARE  LISTING 


The  least  square  calibration  of  the  camera  to  robotic  link  5  (end 
effector)  coordinate  transformation  is  obtained  as  follows: 


X  =  (aWb 

where 

X  -  the  vector  of  least  squares  coordinate  transformation 


constants. 


A  =  the  matrix  of  observed  camera  coordinates  with  12  rows  of 
va,ues  <*cam-  W 

B  =  the  matrix  of  the  x  and  y  position  values  of  the  object 
expressed  in  fifth  link  coordinates  with  12  rows  of  values 
(x5,  y5). 

Thus 


a  c  j 
b  d  ! 

|x0  y^ 


92 


c 

t 


FILE  LSTSOS. t-TN 


R  F  V  I  S  E  P  I?  JUNE  19B4 
AUTHOR  MARK  FURPER 


E  THIS  FILE  CONTAINS  FORTRAN  SUPROUTINESI 

L 

C  lSTSOS 

T  SNVERI 


>  SOME  jUPROUTINES  USE  P  PY  LSTSOS  AND  INVERT  ARE  CONTAINEP  IN  MATH. I 

ClttMMIItlMMIItllTMMtmMMM  t«***tt****tt**t****t*tttt******* 

lUDtutHtKtottuiMiHiittMttmumtttmottMntMMtom 

L  non-recursive  least-squares  suproutine 

r.  sol vc s  a'mxN)  t  x;nxij  =  p  <  m  x  1  > 

SUPROUTINE  LSTSQS<A.X.prATrATA.ATAl.ATAlAT.ASYM.M.NrF'rIER> 


INTEGER  M.N.F 

*CAL  h  <  M  *  N  )  r  XfNrFFt  P(MrF’) 

AfAL  HT(N,K),  ATA(N>UJ,  AML(N.lO>  AIA1AT(N.M>.  AS  Y  M  (  N  *  (  N  +  1  )  /  2 

INTEGER  TCR 

ERROR  COPE  RETURNED  to  1VNERSION  ROUTINE 


CALL  MATTRNIATr  A.  Nr  M ) 

CALI  MATMVJLiATa.  Alt  A,  N.  Mr  N) 

lALL  SNVERKATAI.  ATAr  ASYM.  N,  IER) 

IF  <  IER  .  NE  .  0  )  GOTO  9  9". 

IOO  LALL  MATMIJL  (  A  T  A  1  A  T  .  ATA1.  AT.  Nr  Nr  M) 
LALL  MATMULIX.  A  T  A  1 A 1  .  p.  Nr  Mr  F’ I 


999 


RETURN 


94 


ct«t*tt*tt*tt*tt*ttt*tttttt**t*>ttt**ttttt*t****$*ttt*******t**t**t** 

C  SYMMETRIC  MATRIX  INVERSION  SUBROUTINE 

OOOl  SUBROUTINE  SNUERTiAl.  A.  ASYM.  N.  IER; 


.'002  INTEGER  N 

0003  REAL  A(N.N>.  A1IN.N; 

>004  KE  Al  ASYM < N» < NT  1  >  / 2 > 

0005  INTEGER  IER 

"006  REAL  Ill.  IT 

000'  INTEGER  t  ,  .1 


c - 

0006  lEk  -  0 


c 

CREATE  IDENTITY  MATRIX  IN  Al 

OOO'l 

DU  100  1-l.N 

oo  l  o 

DO  110  J- 1 . N 

00  l  1 

A  I  <  1  »  J  >  -0 . 0 

00  1  0 

1  1  0 

CONTINUE 

00  1  3 

100 

CONTINUE 

0  0  I  4 

DO  120  I  =  I  *  N 

OO  1  3 

A  1  (  1  i  I  )  =  1  .  () 

0  016 

r  j 
o 

CONTINUE 

oo  l  r 

CALL  MA TS YM < ASYM . A . N / 

0018 

CALL  LEQT 1RTASYM*  N.  N.  Al 

0  0  Tr 

RETURN 

0020 

END 

95 


jOOl 

'j  00  2 

uOO  3 

0004 

0005 

0006 

0007 

0008 

0009 

oOlO 

■JO  i  1 

>0  1  2 

001  3 

00  1  4 

0015 

.>0  16 


I  THIS  Mil  •  OMlAiHS  ?Mf  ROBOT  r  A  M  E  R  A  CAL  I  PRAT  I  PN  FROGRAN 

t. 


,  (iitMitMimMinm  titMMMmtMttMtmmuttumuiMtttMMt 
t  t  t  t  1 1 I  I  Y  I  t I M  ft  I  M  <  >  M  I  I  I  l  I  <  t  ( I t  t  n I t I  1 1 M It  f I  1 1 1  M  t I t  t  1 1 M  t  t  M  t  M  t  I  I 


k£  A  l  *■»•')  if  .  .  i  f  N  T  4 

L  SI  A  N  I1  A  f*  [■  IN  (Cf. 

h  £  A  J.  •  L  i 1  R  ’  •  4  f  •  '  .  v  *  B  l  ■  4  '  ,  ►  0  B  1 5  i  4  >  »  r  0  B  J  0  (  4  ) 

C  MATRi*  ..  0  N  f  A  !  N  J  n  0  ^  f  o  ’  1  ^  0  F  R  n  P  P  l  F  COORDINATES 

h  E  A  L  F  L  A  S  P  C  4 

POSITION  of  l  A  S  t  p  IN  i  A  m  £  R  A  COORDINATES  (FIXED) 

Rb  AL  F  I  C  F  TO  i  4  .  1  2  » 

CALIBRATION  F  A  T  TERN 

INTEGER  N  RU  b 

C  NUMBER  OF  BUBBLES  FOUND  BY  SBUB 

R F AL  F  F I  CO ( 4  )  »  THE  T A ( 5  ; 

REAL  r05<4»4>»  FlCT05(4»4)»  F'  I  C  T  50  (  4 . 4  )  ♦  UBJT05(4»4) 

REAL  A(12*3)»X(3«2> »  B  t 1 2  »  2  ; 

REAL  AT<3.l2)»ATA(3»i>tATAI(3r3)»ATAIAT(3»12)»ATASYM(6) 

C  FOR  LEAST -SQUARES 

REAL  AXHAT (  12*2)  rMSERRX  p MSERR Y  p I T S T  < 3 , 3  > 

C  FOR  CALCULATING  MEAN  -  SQ AURE  ERRORS  AND  MATRIX  INVERSION 

INTEGER  STAT 

C  STATUS  WORD  RETURNED  BY  C AM AL 

INTEGER  IER 

C  ERROR  CODE  RETURNED  BY  L  S  T  SOS 


INTEGER  I . I  I . J. 10. 1 10 
C  LOOK  COUNTERS 

REAL  NUAIT 

C - 


DATh 

FH0ME0/90 .0.  0 

Or  60  »  0  r 

1  .0/ 

DATA 

F1CFT0/  0.0. 

-12. Or 

0.  Of 

1.0. 

1 

12.0. 

o 

c 

0 . 0  f 

1.0. 

1 

0.0. 

12.  Or 

0. 0  f 

1.0. 

1 

-11.0. 

0.0  t 

0 . 0  f 

1.0. 

1 

7,79. 

-  4 . 5  » 

0 . 0  f 

1.0. 

96 


l 

7  .  7?  r 

4.5. 

0.0* 

1.0, 

1 

-7.7 9. 

4.5. 

0.0* 

1.0. 

1 

-7 . 79* 

-4.5. 

0 . 0  * 

1.0. 

1 

3.0* 

-5.20. 

0.0* 

1.0. 

L 

3.0* 

5.20, 

0.0* 

1.0, 

1 

-3.0* 

5.20. 

0.0* 

1.0, 

1 

-3.0* 

-5.20, 

0.0* 

1  .  0/ 

001  2 

L - 

NR  I T  E  <  5  *  4  > 

00  1  8 

4 

FORMAT ( / '  CALIBRATION  ROUTINE  '/ 

»0  1  9 

WRI TE  <  5  *  5 ) 

o  o  r  o 

5 

FORMAT ( / '  ENTER  X-T  COORDINATES  OF  OBJECT  (IN  CM): 

1  '  X  =  '  t  / 

'  / 

0  0  2  1 

REM»(5.*)E0BJ0(  1  ) 

u0  2  2 

w  R  I  T  E  (  5  *  6  ) 

00  2  3 

6 

FORMAT (  '  Y  =  %  ) 

0024 

REA  [i(5»  *  >  F‘  0  B  J  0  (  2  ) 

o 

o 

f  J 

POD  JO! 3 »  =  -90.0 

0  0  26 

FOB JO  < 4  )  =  i  .0 

002  " 

UK  1  TF.  (  5  *  7  i 

0028 

7 

FORMAT ( '  NUMBER  OF  SECONDS  TO  WAIT  =  '*> 

0029 

READ ( 5 , * ) NUA I T 

L 

MOVE  TO  HOME  FOSITION 

00  JO 

CALL  MOVE ( FHOMEO . T05 . THETA. ST  AT ) 

00  31 

c 

FIND  LOCATION  UF  LASER 

Wk I TE ( 5 .8  ; 

■'032 

00  3  3 

00  3  4 

00  35 

8 

FORMAT !  '  FINDING  LASER'/) 

CALL  SBUB(FBUBL.  NBUB.  3> 

I F (  NBUB  .NE.  1  )  GOTO  999 

i  A  L 1  MATMOV  <  FLASRC ,  PBUBCd.l) 

c 

GET  PARTIAL  DATA  IF  ANY 

00  3  o 

J  -  0 

0032 

WRITE (5.9) 

J0  38 

9 

FORMAT (  '  ANY  PARTIAL  DATA  IN  CALDAT.DAT  FILE  ! 1 = YE S , -  1 = NO ) * 

.  0  59 

R  E  A  D  <  5  ,  *  >  I  I 

0040 

IF (  I  I  . NE .  1  )  GOTO  20 

004  1 

UK  I TE ( 5 , 29 ; 

00  4  2 

2  9 

FORMAT!'  PREVIOUS  DATA  /> 

0043 

CALL  ASSIGN! J»  ' SY 1CALDA  T . DAT '. 13 ) 

c 

READ  IN  DATA  UNTIL  EOF 

3o 

RE  AO!  3,  9  9  .  f  NO  -JJ)MJMiI>»rtCJH,2)»A(JH.3>»B!JM.I>,RlJ+l. 

o  0  «  5 

004a 

J  =  J  f  1 

0  0  4  7 

GOTO  30 

0048 

33 

CLOSE!  V  N l T  -  3  , 

JO 


J  f  1 


CALL  ASS  I  GNi.ii  3t  !  CAL  DA  r  .  DA  T  *13; 


f-lace  OBJECT 

CA!  L  MOVE  <  FOB  JO. OB J  T 05 »  THE  T A  *  S  T A  T  ; 


U  R  I  T  t  (  5  »  5  0  '  1 0 

}  FORMAT (  .  JO  -  »  [ 4  /  '  PLACE  WHITE  DOT  THEN  ENTER  1  [CRT:  *> 

R  F  A  ti  (  5  *  *  >  I  I 

IF (  I  I  .  NE .  t  *  00  TO  099 


I  F  .  j  >.)  .HE.  t  2  •  GOTO  4  00 
DO  400  J=  JO  *  17 


MOVE  10  FOSITION  AT  WHICH  TO  TAKE  PICTURE 

UR l T  E  <  5  *  3  7 ) J 

FORMAT (  *  J  =  .14/) 

CALL  VCADD4(  PPICO.  FOBJu*  P  I  C  F  T  0  (  1  »  J  )  ) 

CALL  MOVE (PP ICO. PICT05*  THETA. ST AT  ) 


«  A  t.  l  UA  I  T  (  NWA  I  T  j 

?  CALL  SBUB(FRUBC»  NBUB*  3) 

IF  (  N BU B  .  EW.  2)  GOTO  55 
UR  I TE ( 5 . 57  ) 

FORMA M’  TRYING  AGAIN  '  /  ; 

GOTO  49 


DISTINGUISH  BETWEEN  LASER  AND  OBJECT 
IF  (  VECEOLK  F  I.ASRC  *  F  BUBC  <  1*1  )  .  4  ,  4 . 0  )  » 

1  CALL  MA T MOV ( F OB JC * F BUBC <  1  *  2 > . 4 .  1  > 

I F  < V  E  C  E  0  U  < PL  ASRL  . F  BUBC (1*2). 4*4. 0>/ 

1  CALL  MA T MOV < F OB JC » F BUBC (  1  *  1  )  *  4  *  1  ) 


UF  I  TE  '•  5  »  5  6  )  FOB  JC  <  1  »  *  PO  B  JC  '  2  )  »  PO  B  JC  (  3  )  *  PO  B  JC  (  4  » 

FORMAT  <  /  /  POSITION  QF  OBJECT  IN  CAMERA  COORDINATES  -  4P5.0* 

TRANSFORM  TO  COORDINATE  SfSTEM  5 
CALL  I NVT05( F  IC T05»  P1CT50) 

CALI.  MA  T  MUL  ■  POB  J5  »  F  I  C  T  5  V  ♦  POBJV*  4*  4.  1; 

ADD  THIS  FOSITION  TO  DATA  ARRAYS 
A  <  j .  1  ,  =F OB JC (  1  i 
J. 2  )  -  ROB JC C 2  ) 

A  i  Jt  l  >  -RGB JC  (  4  ; 

B(  J*  1  ) =F OB J5 (  1  ) 

B '  J  »  2 ) =POB J5 i 2  » 


>  o  1  '' 


if-  ■  A  B  S  [I  f  L  T  A  <  )  .  !  r  .  2  1  .  A  N  1 1  .  , A  b  r  ■  iTL  T.V 


■  t  i>  o  t  n 


c  is  t  - o  .  o 

►  if*  [i  i  he  .’ELOCiTr  ur  inject  in  i  mace  run 

UE  LX  2  =  DEL  *  2  TIME 
■JEL  r  2  =  DEL  f  2  /  TIME 


i:  NOW  ESTIMATE  WHERE  THE  THIRD  OBJECT  SHOULD  Bt 

e  ?  t  v  1 3  =  f- pu b  ( i » i  )  fuel  x :: *  (  2  *  t  i  me  > 

rr.  TX23=RBUR  (  1  «  2  >  WEI  <2«  T  I  ME 
tST M3  =  FBUB< 2.  I  )FUELY?*(2*1  I  ME  > 

E  S  T  Y  2  3  =  F‘  B  U  B  '  2  •  2  >  W  E  L  Y  2  t  TIME 
tAU(  1  >  =  <  E  S  T  *l3+ESTX23)/2 
t  A  V  (  ?  1  -  (  E  S  T  Y  1  ?■  F  E  S  T  Y  2  3  )  /  ? 
tERX3=RBUB  (  1  »  i  >  -fc'A V  (  1  ) 

ERRy  3  =  F  DUD  (  2  »  3  »  --EAU  <  2  > 

UEUElOFE  tSTIMAT  ION  [  NF  0  FOR  FOURTH  OBJECT 

IMAl»-(ERRX3*(  T  IME2/T  IMG  )  )  **24-  (  ERRY3*  <  TIME2/TIME>  >  **2 

J  E  C  3  =  S  0  F  T  *  FL  OAT ( I M AG )  ) 

IF<ERRXi.EU.O)GOTO  ion 
aNG2  =  A1AN(FL0AT  f  E  R  R  Y  3 / E  R  R  X  3  >  > 

GOTO  110 

100  hNG  2  = i .  14/? 

FINE'  THE  VELOCITY  OF  OBJECT  IN  IMAGE  THREE 

00  3  S  MU  VE LX  3  =  DEL  X  3  /  T  IMF 

>03*  v'  E  L  Y  3  =  D  E  L  Y3/  TIME 

I'..  NOW  ESTIMATE  WHERE  THE  OBJECT  WILL  Bt 

■0  3  2  tS  T  X 1 4  =  RBUB (  1  » 1)  +  ( VEL  X2f UELX3 ) *  T IME+ ( UELX2  FUELX3  )  /  2*  T  I  ME? 

0  0  3 8  ESrx:M  =  F  BUB  (  1  .  ?)  WELX3*T  IMEF  WELX2WGL  X3  >  /2*T  I  ME? 

>0  39  tSTX34  =  RBUB<l*J>  +  '.  VELX?WELX3>/2*TIME? 

00  40  F  STY  1  4  =  PBUB  <  2  .  1  >  4  (  UEL  Y'1  WEI  Y  3  )  *  T  I  ME  f  '  UEL  Y2  WEL  Y3  )  /2*  T  IMF? 

•0  4  1  tST  Y24=F  BUB( 2. 2 >  WEL Y3*T IMEf ( VELY2WEL  f 3 ) /2*T  I  ME  2 

00  4L  FSrYJ4=RBU&(2  i3)  +  WELY2WFLY3>/?*TIME: 

"'043  t  AU(  3  )  =  (ESTX1  4  FES  TX24  +  ES1  X34  )  /  3  +  UEC3*COS  f  ANG2  ) 

00  4  4  E  ALM  4  >  =  <  E  S  T  Y  1  4  f  E  S  T  Y  2  4  F  E  S  T  Y  3  4  >  /  3  F  OE  C  3  *  S  I  N  <  ANG  2  / 


00  4  s 

GOTO  *00 

0  4  .*> 

SOo 

DU  6  30  1  =  1. 

0  0  4  ' 

r  C  AM '  I  •  r  F  B  U  B  f  I  »  i  1 

0  0  4  8 

6  30 

r nn t i n ijf 

■>04? 

►  C  AM  (  1  )  -  -  P  r  am  -  l  *  *  .'  i 

2  S  6 

00  SO 

R  C  A  M  '  2  ■  -  <  F"  F.  A  M  •  2  ?  *  2 

2  4  <  i 

•>05  1 

.AIL  F  OF  F -t  F r  AM  .  f  OB 

>  s  •  f  r  ij  r  ,  i 

r  o  s 

0 02  t 

‘'0  30 
0031 
,0  32 
•:  o  3  * 

)  ?  3  4 


•0?l 
"*022 
>02  3 
o  024 
• )  0  2  S 
0  0  2  6 
'022 
00  2  8 


*  0  1  > 


u 

I 

I 


no 


I  L  E  kF  T['AT  .  -  T  M  R  E  I  S  E  B  19  3EF  TEMPER  178  4 

rms  file  contains  >  hf  subroutine 

R  F.  I  l*  A  1  .  t-  IN 


'O  1 


'00  2 


M  0  0  4 


■  >  o  ■: r 


")  o  r.  . 

’)o: 


'00  9 

'■  A  O  V 


)  1  1 
•oi. 

1 0  1  3 
0014 

)  r}  J  c 

0  0  t 


UMMlttlHIMUMt 

C***«**t**J 

SUBROUTINE  KETPAT (  T  IMF . 
C  THIS  S I !  F»  k  0  U  T  INF.  UlLi.  PROVIDE 


i  IMF  Of  F BUB » P . RRE V . PCUR  .  I  0  0  *  F'  C  A  M  ,  F  OP  J  j  »  C  ) 
AN  ESTIMATE  Ok-  THE  FOSITIJN 


OBJECT  AT 
■El  V  2  .  VEL  r  _• 

.  S  T  X  1  3  .  E  T  Y  1  1 


tST<?3»fST/23 


JF  THE  OBJECT  AT  SOMF  SFECIEIC 

' E  l. oc  i  r  i  Cb 

-  tSTIMATF 
f  OS  I r I  On 

-  ESTIMATE 
FOS  I  T I  UN 

INTEGER  VE L  <  2 . vE  L  Y  2 . t  S  T  x 1 3 . F  S  T  * 2  3 . L  S  T  Y 1 3  *  E  ST  Y  2  3 
EAV<1;  -  ESTIMATE  FOR  OBJECT  AT  POSITION  3 
t  R  R  X  3  *  E  R  R  Y  3  -  t  R  R  0  R  BETWEEN  ESTIMATE  A N B  OBSERVATION 
F  OS I  T I  I  ON  3 


POINT  IN  TIME 
BETWEEN  IMAGES  1  AND 
OF  OBJECT  IN  IMAGE  3 
I  N  I  MAGE  1  AS  INITIAL 
OK  OBJECT  IN  IMAGE  i 
IN  IMAGE  2  AS  INITIAL 


USING 
T  OG  I  T  I  ON 
US  I  NG 
POSITION 


INTEGER  E  A V ( 4 )  , E  RR  X  3  »  E  R R  Y  3 

THESE  ARE  THE  ESTIMATE  UF  POSITION  4  USING  DIFFERENT 
INITIAL  KOSI T IONS 

INTEGER  ESTX14,F_STX:’4.ESTX34.ESTY14.ESTY24,ESTY3  4 
VEL X  3  *  vT  L  Y  3  -  VELOCITY  VALUES  BETWEEN  IMAGES  2  AND  3 

INTEGER  V  E  L  X  3  »  V  E  L  Y  3 


THESE  ARE  I  HE  POSITION  CHANGES  BETWEEN  I  HE  FRAMES 

INTEGER  PELX2 , HELX3 . BEL Y2  t  DELY3*  PEL  TAX . PELT  AY 
INTEGER  C 

nEAL  TOS ( 4 . 4  > , KPEV  <  4 )  ♦ kCUR ( 4 )  » PCAM  (  4 )  *  kOB J5 (  4  ) 
REAL  KHUB(4.4>.F(4)»TIMF_,TIME: 


INITIALIZE  LON ST  ANTS 


L  ‘  i  . 

KIND  THE  POSITION  CHANGES  BETWEEN  IMAGES  1  AND  2. 
r  ANB  ALSO  BETWEEN  IMAGES  2  AND  3 


ii  E  L  X  2  =  F  B  U  B  '  1  *  2  )  -  R  B  U  B  i  1  ♦  1  ) 
BEL  Y  2  =  P  DU  B  (  2  »  2  ;  -  P  B li  B  '  2  .  i 
i»ELX3-FB0B(  1  •  3  )  -RBUB  <  1  .  2  > 
BELY3-FBUB(2»‘3'  F  BUB  (  2 . 2  » 
i»E L  T  A X  =  F  BUB  <  1  »  J )  -  F  BUB  1  1  *  I  > 
BEL  T  AY  -  PBUB  '  2  »  3  »  f  B'JB  •  :  •  1  ■ 


lITTLE  CHANGE  fRE»«*  AS  r 0  ■  HANCO 
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00  38 

253 

no  40  I  =  r CENT  -  COUNT Y .  YCENT+COUNT Y 

0039 

I z Z  =  RF IXEL(XCENT+XLTH/4,I / 

0040 

JF  (  IZZ.EQ.O.'GOTO  40 

004  1 

COUNT2=COUNTZ+t 

0042 

L  OS  T  Z  s I 

004  3 

40 

CONTINUE 

00  4  4 

IF (COUNT Z . L 1 . 3 >GOT 0  254 

0045 

YCENT2  =  L«STZ - (COUN  rz/z  ■ 

0  0  46 

25  4 

TCENT*<  TCENl t  rCENTl frCENT?>/3 

00  4  7 

RETURN 

00  4  9 

END 
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C  *t*M*»***ttt*»«»t***t*t********t*t*ttttt*tttttt**tt*ttt* 
C  «***»****»*♦»*«**»**♦«** *****t**««***»**«*****»********»* 


'001 


0002 

>003 

0004 


0  00  3 
0  0  0  6 


0007 
0008 
0009 
00  10 


001  1 
.1012 
001  3 
00  1  4 
001  3 
0016 
00  1  ' 
00  18 

00  19 
00  20 
0021 
00  2  2 
002.3 
00  2  4 
00  23 
0026 


0027 


0028 

0029 


.'030 
003  1 
0032 
0033 
00  3  4 
0033 
00  36 
0037 


C  IHIS  ROUTINE  IS  USE  8  TO  PROVIDE  8  MORE 

C.  ACCURATE  ESTIMATION  OF  THE  OBJECT'S  CENTER 

SUBROUTINE  ACE NT ( XCEN1 .rCENT. COUNTY. XLIH) 

INTEGER  XCENT , YCENT , XL TH .C0UNT1 

INTEGER  RF'I  xEL  »  XCENT  1  »  XCENT2*  YCENT  1  r  YCENT2 

INTEGER  C0UNT1  .  C  OUN  T  2  .  L  AS  T  1  . 1.  AS  T  2 

C  INITIALIZATION 

COUNT  1 =  0 
C OUN  T  2  =  0 

XCENT 1 =XCENT 
XCENT2=XCENT 
YCENT I  - rCE  N I 
TCENT2=  rCE  NT 

DU  TO  I-XCEN1  -XI.TH,  XCENT  fXLTM 
IZ1 =  RFI XEL (  I . YCE  NT -COUNTY/4 ) 

I F ( IZ1 .EU.O IGOTU  10 
COUNT  1 =COUNT 1 F 1 
LAST  1  =  1 
10  CONTINUE 

IF ( COUNT  1  . L  I  .  3 ;GOf 0  251 
XCENT1 =LAST 1  - <  COUNT  1 /2> 

251  DO  00  I  =  X  C  £  N  T  -  XLTHt XCENT +  XLTM 
I7  2  =  RPIXEL ( I . YCENT  4  COUNT Y/4 > 

IF ( IZ2.EU.0)G0T0  20 
C0UNT2=C0UNT2F1 
LAST2=  I 
20  CONTINUE 

IE (C0UNT2.LT .31G0T0  257 
XCENT2=LAST2- (COUNT  2/2 ) 

25  2  XCEN1 = <  XCENT  FXCENT 1 FXCENT2  > / 3 

COUNT  1=0 
COUNT  2  =  0 

DO  30  I  =  rCENT-COUNTY  »  YCENT  FCOUNTY 
I/l=REIXEL<XCENT-XLfH/4.I> 

I E  (  I  Z  1  .  E 0 . 0  )  GOT 0  JO 
coun  r i =count  i  + 1 

1 AST1=I 
30  CONTINUE 

IE ( COUNT  1  . L 1  . 3 >G0T0  253 
YCENT  1=L ASTI  - (COUNT l / 2  ■ 


r 


i 


Tar 
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£  AC  H  SECT ION< 256X240  PIXELS) 


oo  i : 

300 

DU  210  Y= 02*240*239+ 02*240* 10 

0013 

00  220  X= 01*256. 255+01*256*5 

001  4 

Z  =  F:F'  I  XEL  <  X  .  Y  / 

0015 

I F  <  Z . NE . 0  >  GO  10  215 

0016 

GU  TO  220 

■J01  7 

215 

T  E  M  F'  X  =  X 

0018 

T  E  M  F1  Y  =  t 

0019 

00  230  Xl= TEMF X. TEMFX+40 

00  20 

Zl-F1  FIXELCX1  .  TEMF'Y  , 

0021 

I E  c  Z 1 . NE . 0 ) GO  TO  230 

0022 

I  E  (  X 1  . L T . TEMFX  +  2 ) GO  TO  220 

0023 

XLTH=Xl- TEMFX 

o 

o 

f-j 

-t> 

XCENT  =XL  TH/2+  TEMF'X 

0025 

GO  TO  240 

002a 

230 

CONTINUE 

0027 

240 

00  250  Yl=?EMRY-<XLTH/2>-15.TEMFY+(XLTH/2>+15 

0026 

Z2=RFIXEL<XCENT  *Y1 > 

0029 

IF(Z2.E0.0)G0  TO  250 

0030 

COUNTY=COUNTY+ 1 

0031 

l ASTY=Y1 

0032 

250 

CONTINUE 

0033 

IF(COUNTY.LI .2)G0  TO  251 

0034 

YCENT  =  LASTY - <  COUNTY / 2  * 

0035 

CALL  ACENTTXCENT. YCENT. COUNTY. XLTH) 

003a 

OR  t INO) =  X  C  £  N  T 

0037 

OR ( INO+1 >= YCENT 

0036 

I N  0  =  I  N  0  ♦  2 

003V 

NBUB=NBUB+ 1 

0040 

COUNTY  =0 

004  1 

IFfNBUB.EQ. 1 >GOTO  102 

0042 

IF<NBUB.EG.2)GOro  100 

0043 

IE < NOUS . EG . 3 ) GOTO  450 

0044 

251 

COUNTY  =0 

00  4  5 

220 

CONTINUE 

004a 

210 

CONTINUE 

0047 

102 

CONTINUE 

0046 

100 

CONTINUE 

C  BUILD 

K  BUB  MATRIX 

0049 

450 

OU  830  1  =  1  *  NBU B 

0050 
005  1 
0052 

0  0  5  3 

PBUB(  1 . I ) =ELOAT  <  DR<  2*1 -2) ) 
F  PIIB(  2*  I  )  =FLOAT  (  0R(  2*1  -1  )  I 
FBUBT3. I )=0.0 

F'BUB  <  4.  I  )  -1 .0 

0054 

830 

CONTINUE 

r  i  l  e  f  3  « .  r  i  -i 


REVISED  20  AUGUST  1984 


1.  THIS  FILE  CONTAINS  THE  SUBROUTINE 

T  F  3B . F  TN 


T  FILE  F  3  B  .  F  T  N  REVISED  20  AUGUST  1994 


THIS  FILE  CONTAINS  THE  SUBROUTINE 
F  3B . F  TN 


r*********************************************************************** 
C********************************************* ******************** 

SUBROUTINE  F  3B  <  F'BUB  .  NBUB  ) 

C  THIS  SUBROUTINE  LOCATES  THE  POSITION  OF  THE  OBJECTS 
C  WITHIN  THE  FRAME  BUFFER  ANP  REFERS  ALL  POSITIONS  TO 
c  the  top  l e r i  quadrant  .  this 

C  ROUTINE  WAS  UPDATED  10  BE  MORE  ACCURATE  WJTH  THE 
c  center  finding  technique  bt  using  the  subroutine 
C  ACENT'  WHICH  finds  several  values  for  center  LOOR- 

C  DINATES  AMD  AVERAGES  THEM  TO  DERIVE  THE  FINAL  VALUE. 

INTEGER  < . T ,RP IXEL . Z  .  I 

r.  TEMRX.  TEMPT  -  INITIAL  BOUNDARY  COORDINATES 

C  XLTH  -  LENGTH  OF  INITIAL  CHORD  MEASURED  ON  OBJECT 

INTEGER  TEMPXfTEMRY.21.Z2, XL  Th 

C  COUNTY  -  LENGTHH  OF  INITIAL  CHORD  IN  Y  DIRECTION 

C  LASTY  -  LOCATION  OF  LAST  OBJECT  PIXEL  ON  INITIAL  CHORD 

2  XCENT ■ YCENT  -  ESTIMATES  OF  CENTER  COORDINATES 

INTEGER  XCENT . YCENT .COUNTY .LAST Y 


C  DR  -  ARRAY  CONTAINING  CENTER  COORDINATES 

C  IMP  -  INDEX  TU  SPECIFIC  VALUES  IN  DR  ARRAY 

C  NBUB  -  NUMBER  UF  OBJECTS  FOUND 

INTEGER  DR(0: 10) . IND.NBUB 
INTEGER  Y1.X1.D1.D2 

C  F  BUB  -  REAL  ARRAY  CONTAINING  OBJECT  VECTORS  IN  CAMERA  COOR 

REAL  RBUB ( 4 . 4  i 

C  INITIALIZE  CONSTANTS 

iV  R I J  R  =  0 
l  N  D  =  0 

C  START  PROCESSING  I  HE  FRAME  BUFFER 


SUBROUTINE  DELAYCN) 

C  SUBROUTINE  DELAY  will  provide  a  DELAY  of  N  sec 

'<002  7NTEGER  L  .  0 

ooos  real  n 

•'004  rt=-<0»N 

0005  nu  S  l  - 1  •  rt 

>006  lALL  boh 

ooo:  i  continue 

•  >008  kETURN 

OOOS  EN 
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THIS  SUBROUTINE  WILL  PRODUCE  THREE  PICTURES  IN  A 
C  SINGLE  FRAME  BUFFER . 

.1001  SUBROUTINE  I  SN  APS  <  T  I  ME  .  T  I  ME  2  .  P  BUB  .  NBUB  ) 

OOOi  INTEGER  NBUB 

J003  REAL  I IMEf TIMEO.PBUB 

C  INITIALIZE  CONSTANTS 

C  TAKE  THREE  PICTURES  ‘TIME*  SECS.  APART  AND  STORE 
I.  IN  FRAME  BUFFER. 


,»004 

L  ALL 

KAN ( 0 ) 

0005 

CAl  L 

SCROLL ( 0 i 

.>0  06 

CAL  L 

ZOOM ( 1 » 1 ) 

ooo : 

CALL 

SNAP 

.>009 

CALL 

KAN ( 256  > 

OOOS 

CALL 

VIEW 

,)0  10 

l  ALL 

UELAY < TIME ) 

00  1  l 

CALL 

SNAP 

joi  z 

i.  ALL 

KAN (  0  > 

0013 

CALL 

SCROLL ( 240 

JO  1  4 

..  ALL 

VIEW 

00  1  r. 

CALL 

[iELA>  <  TIME 

0  0  I  6 

l  ALL 

SNAP 

001  ■ 

call  pan;0' 

>018 

CAL  L  SCRQL  L  <  0 l 

0019 

50 

CALL  F3B  (  F  BUB • NBUB 

oo:o 

RETURN 

OOZL 

END 

)  0. 3  8 

v  ALL 

rSNAF-S(  TIME,  riHE2.FBUB.NBUB) 

00  3  S 

CM  L 

RETtAI.TIMt.riMC2.FBUB.F-.fREy.FCUk.T05.FCAH 

'040 

*  F  *  C 

.  FO. 0 . U )GOTO  t  BO 

004  1 

HU  1 

’5  J=t.4 

00  4  2 

r  0  <  J  >  =  K  R  E  V  (  J  > 

00  4  • 

1  "5 

COM T I  HUE 

>0  4  4 

JOTO 

1  90 

0045 

180 

CALL 

FORE ( F  »  f  5  »  F 0  » T05  > 

JF  ft  A  T  E 

FREV  VALUES 

D04r. 

F  REV 

(  1  /  ~  F'  0  (  1  , 

>04  ^ 

.v  R  E  V  <  2  >  -  F  ‘  0  <  2  ) 

004&  190  CAl.l  MOVE  <  F  o  •  T0t»»  THE  TA*STAT  j 

>049  .'.ACL  WAIT(TIME) 

0050  200  CONTINUE 


PACKAGED  30F TUARE ( IMAGE  TECHNOLOGY > INC . > 


CAI  L  SELGRF  <  1  ; 

CALL  JYNC(O) 

CALL  FBINIT 
u ALL  AFINIT 
CALL  LUINIT 
.ALL-  UIDCHNTO) 

INITIALIZE  A  BINARY  L U T (  ' T HRE S ' = 0 » ML  A C K  ) 

WRI TC( j. 999 , 

?99  FORMATdX,'  PLEASE  INPUT  THRES  FOR  ZERO  INTENSITY') 
READ! 5. 998)  THRES 
>98  FORMAT (13) 

CALL  SELLU  T (0*0; 

HO  1  I -0. THRES 

CALL  SETLUTd.O, 

1  CONTINUE 

HO  '2  i  -THRESt 1 , 255 
CALL  3ETLUT  <  I ,255 ) 

2  CONTINUE 

SELECT  OUTPUT  CHANNEL  AND  UIEU 

CALL  SELLUT ( 1,0, 

CALL  DIED 

BUILD  INITIAL  T  05  MATRIX 

CALL  MOUECPHOME. T05, THETA, STAT ) 

INFU1  TIME  BETWEEN  INITIAL  SNAFSHOTS 
WRI TEL  5. 99) 

99  FORMAT d X INPUT  f  I  ME  d  N  SECS.)  BETWEEN  IMAGES.') 

REAP ( 5 , 10 ) TIME 
10  FORMAT (F10 . 4 ) 

INPU1  TIME  ESTIMATION  POINT 
WRITE (5, 98) 

90  I  ORMAT ( IX,  ' INPUT  T I  ME ( I N  SECS.)  FOR  FINAL  POSITION. 
READ(5,10)TIMEZ 

INITIALIZE  ROBOT  POSITION  FUR  CATCH  UP  MODE 

PREVI 1 ) =90 . 0 
I'REU  ( 2  )  =0 . 0 
PRFV ( 3  )  =60 . 0 
PRE V ( 4  >  =  1 .0 


START  TO  TRACK  OBJECT 


APPENDIX  B:  COMPUTER  PROGRAM  LISTING 


OOot 


,HIS  PROGRAM  CONTROLS  THE  IMAGE  PROCESSING  AND 
KOPDlIi;  MOVEMENT  FOR  A  SIMPLL  TRACKING  APPL 1  - 
'AT  ION  OF  THE  IS  INDUSTRIAL  ROBOT.  THE  SYSTEM 
IMPLEMENTS  A  TYPE  01  VISUAL  CONTROL  BY  EXTRACTING 
POSITIONAL  DATA  THROUGH  THE  USE  OR  AN  IMAGE 
ACQUISITION  SYSTEM. 

.AST  REVISED  ON  DECEMBER  05.1984 

MAIIRT  CL  !.  O  DONNCLl 

NECESSARY  VARIABLES 

GTA1  FLAG  TO  SIGNAL  PROPER  COMPLETION  OF  MOVE 
'BUB  VALUE  OK  NUMBER  UF  OBJECTS  FOUND- 
THR-Eb  -  THRESHOLD  VALUE  FOR  BINARY  IMAGE 
Z  FLAG  DENOTING  CATCH  UP  ROUTINE  IN  EFFECT 

I M TEGCR  S 1  AT •  NBUB ■ THREE  »C 

1  I  ME  -  TIME  PERIOD  BETWEEN  SUCCESSIVE  IMAGES 
IlMC.’  -  ESTIMATION  TIME  PERIOD 

,-HOME  -  ARRAY  CONTAINING  1  HE  'HOME'  VECTOR  FOR  P5 

-  GENCRAL  TRANrORMAT ION  MATRIX  FROM  LINK  5 
COORDINATES  10  BASE  (0)  COORDINATES 
T C'J  (4.4;  -  GENERAI  TRAMSF  ORMAT  ION  MATRIX  FROM  CAMERA 
COORDINATES  TO  LINK  5  COORDINATES 


0002 


C 


REAL  TIME.  T  I HE Z .PHONE  (  4  )  ,  T05<  4 .4>.TC5<4.4; 

THETAtS)  -  hRRAY  CONTAINING  THE  FIVE  ANGLES  WHICH  DISCRIBE 
THt  ORIENTATION  AND  POSITION  OF  THE  P5 
F  REV  -  ARRAY  UF  VALUES  THAT  HOLD  THE  PREVIOUS  POSITION  Or 

THE  ROBOT  IN  BASE  COORDINATES.  THESE  VALUES 
ARE  USED  WHEN  THE  OBJECT  IS  MOVING  VERY  SLOW 
PCUh  -  ARRAY  USED  BY  SUBROUTINE  TO  STORE  CURRENT  POSITION 
OF  OBJECT  IN  BASE  COORDINATES 

P C  AM  -  ARRAY  USED  BY  SUBROUTINE  TO  HOLD  LAST  IMAGE  COORDINATES 
P0BJ5  -  ARRAY  RC AM  TRANSFORMED  INTO  LINK  5  COORDINATES 
PU  -  THIS  ARRAY  CONTAINS  THE  ESTIMATED  POSITION  OF  THE 

OBJECT  AT  THE  ESTIMATION  TIME  IN  BASE  COOR¬ 
DINATES 


JQ03  ..'EAL  THETA<  5)  .RCUR(  4  )  »P0BJ5<  4  )  .FCAM<  4  >  .PREV<  4  )  »P0<  4  ) 

C  PBUB ( 4 » 4  J  -  ARRAY  CONTAINING  THE  CAMERA  COORDINATES  OF 

C  'HE  MOVING  OBJECT 

C  rj  <  4  '  -  ARRAY  CONTAINING  THE  ESTIMATED  POSITION  OF  THE 

C  MOVING  OBJECT  IN  LINK  5  COORDINATES 

C  P(4i  -  ARRAY  CONTAINING  THE  ESTIMATED  POSITION  OF  THE 

C  MOVING  OBJECT  IN  CAMERA  COORDINATES 


0004 


REAL  PBUB < 4 • 4 . ,P5< 4 ) . p< 4 < 


.NITIALIZE  CONSTANTS 


00  0  5 


DA  I  A  P HOME/90. 0.0. g.60.0* 1 .0. 


INITIALIZE  IMAGING  EQUIPMENT  USING 


100 


98 


KRINT  our  UPDATED  DATA 
DO  90  J J -  1 .  1 2 

URITE<5.9t)A<JJ.l>.A<JJ.2>.A(JJ,3>.B<JJ.l>.B<JJ,2) 

FORMAT  (  '  .  3F10  .5 .  7X  ,  2F1  0 .5  ) 

uONTINUE 

SAUE  IN  PARTIAL  RESULTS  FILE 

URITE<3.99>A(J.1>.A<J»2>.A<J.3>.B<J.1>.B<J.2> 
FORMAT (  '  ' . 5F 15 . 8 . 


LON  1  INUE 


UR  I TE<5. 17 ; 
FORMAT ( / /  ) 


CAlL  n0VE<FH0ME0>  T05.fHETA.STAT ) 


SOLVE  FOR  COEFPIC IENTS 

CALL  LSTSQSCA. X. B.AT , OTA. OTA 1 . rtl A1AT . A TaSYA . 12>  3. 2  . IER  i 


OUTPUT  COEFFICIENTS  TO  TTY  AND  CAMCON.DAT  FILE 
URITE<5.200>X<l»l>>X<2>l).X<3.1>.X<1.2)»X<2»2>iX(3.2) 
FORMATT  T5C<1.1)  -  '.F15.8/ 

1  T5C  <1.2)  =  ’  .F 15 . 8/ 

I  T5C< 1.4)  =  ’  »  F 1 5 . 8 / 

1  '  T5CT2.1 )  *  ' .F15.8/ 

1  T  5C ( 2 . 2 )  =  '.F15.8/ 

1  '  T5C (2.4)  =  '  . F 1 5 . 8/ ) 


CALL  ASSIGN(2.'SY: CAMCON . DAT '  .  1  3 ) 

UkITE<2.210)X<1.1).X(2.1).X<3.1).X<1.2).X<2.2).X<3.2) 
FORMAT  <  6  <  F 15 . 8/ > ) 

CLOSET  UN  I  T  =  2  i 


CALCULATE  MEAN-SQUARE  ERROR 
CALL  MATMU LiAXHAT. A. X. 12.3.2) 

MSERRX=0 . 0 
MSER RY  =  0 . 0 
DO  500  11=1.12 

MSERRX  =  MSERRXP<  B<  I  I . 1  . -AXHAT ( I  I . 1 > ) *»2 
MSERRY  =  MSERRY  +  <  B< II . 2 ) -AXHAT  < 1 1 . 2) >**2 
CONTINUE 

MSERRX=MSERRX/12.0 
MSERRY  =  MSERRY/12 .0 
WR 1 TE < 5 . 550 > MSERRX . MSERRY 

FORMATT'  MEAN-SOUARF  ERROR  IN  X  =  '.F15.5/ 
I  MEAN-SOUARE  ERROR  IN  Y  =  '.F15.5) 

CHECK  0U1  INVERSION  OF  ATA  MATRIX 

CALL  MATMULTITST.  ATA.  ATAI.  3.  3.  3) 
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59  U  R  I  T  E  (  5  »  2  6  « 

<50  26  FORMAT! IX,  CATCH  UR  MOt'E  '  ) 

61  GOTU  610 

C  FORM  THE  F  VECTOR 

'62  600  F  <  1  >  =  (  FLOAT  (  EAO  (  3  1  )  *2  >  25t> 

63  P(2>=(FL0AT(EAV<4))*2)-240 

64  R (31=0*0 

6  5  F ( 4 ) = 1 . 0 


iHIS  POUT  I N  E  f  ORMS  THE  NECESSARY  •  R '  UECTOR  10  MOVE  RORGI 

SUBROUTINE  FORME .T5.FO. T05, 

N  E  A  L  r  I  4  )  *  I  0  S  !  4  *  4  >  ,  F  j  4  > 

ASSIGN  LUN  :  TU  CAMCON.RAI 

lAIL  ASSIGN!?,  SY I CAMCON . BAI ' , 1 3 ) 

GET  CAM -TO  ROB  CONSTANTS 

K  E  A  III  2  .  f  )  A  .  B  .  X  0  ,  c  •  P  •  TO 
CLOSE ' UN  IT  =2 i 

p5<  1  )=A*R(  1  )  fR*E(2>  !  <■> 

F  S  (  2  )  =  C*R  (  1  )  +  B*P  <  2  )  +  Y  !i 
f'5  i  3  )  =0 . 0 
F’5(  4  )  =  1 .0 

lALL  rtATMUL  IF'O,  TOO, f'5, 4 , 4,  1  ) 


RETURN 


END 

FILMED 

4-85 
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